add humble-navigation2

navigation2/nav2_system_tests/src/behavior_tree/CMakeLists.txt

@@ -0,0 +1,16 @@
+find_package(Boost COMPONENTS system filesystem REQUIRED)
+
+ament_add_gtest(test_behavior_tree_node
+  test_behavior_tree_node.cpp
+  server_handler.cpp
+)
+
+ament_target_dependencies(test_behavior_tree_node
+  ${dependencies}
+)
+
+target_include_directories(test_behavior_tree_node PUBLIC ${Boost_INCLUDE_DIRS})
+target_link_libraries(test_behavior_tree_node
+  ${Boost_FILESYSTEM_LIBRARY}
+  ${Boost_SYSTEM_LIBRARY}
+)

navigation2/nav2_system_tests/src/behavior_tree/dummy_servers.hpp

@@ -0,0 +1,207 @@
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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. Reserved.
+
+#ifndef BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
+#define BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+#include <chrono>
+
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using namespace std::chrono_literals;  // NOLINT
+using namespace std::chrono;  // NOLINT
+using namespace std::placeholders;  // NOLINT
+
+template<class ServiceT>
+class DummyService
+{
+public:
+  explicit DummyService(
+    const rclcpp::Node::SharedPtr & node,
+    std::string service_name)
+  : node_(node),
+    service_name_(service_name),
+    request_count_(0),
+    disabled_(false)
+  {
+    server_ = node->create_service<ServiceT>(
+      service_name,
+      std::bind(&DummyService::handle_service, this, _1, _2, _3));
+  }
+
+  void disable()
+  {
+    server_.reset();
+    disabled_ = true;
+  }
+
+  void enable()
+  {
+    if (disabled_) {
+      server_ = node_->create_service<ServiceT>(
+        service_name_,
+        std::bind(&DummyService::handle_service, this, _1, _2, _3));
+      disabled_ = false;
+    }
+  }
+
+  void reset()
+  {
+    enable();
+    request_count_ = 0;
+  }
+
+  int getRequestCount() const
+  {
+    return request_count_;
+  }
+
+protected:
+  virtual void fillResponse(
+    const std::shared_ptr<typename ServiceT::Request>/*request*/,
+    const std::shared_ptr<typename ServiceT::Response>/*response*/) {}
+
+  void handle_service(
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<typename ServiceT::Request> request,
+    const std::shared_ptr<typename ServiceT::Response> response)
+  {
+    request_count_++;
+    fillResponse(request, response);
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  typename rclcpp::Service<ServiceT>::SharedPtr server_;
+  std::string service_name_;
+  int request_count_;
+  bool disabled_;
+};
+
+template<class ActionT>
+class DummyActionServer
+{
+public:
+  explicit DummyActionServer(
+    const rclcpp::Node::SharedPtr & node,
+    std::string action_name)
+  : action_name_(action_name),
+    goal_count_(0)
+  {
+    this->action_server_ = rclcpp_action::create_server<ActionT>(
+      node->get_node_base_interface(),
+      node->get_node_clock_interface(),
+      node->get_node_logging_interface(),
+      node->get_node_waitables_interface(),
+      action_name,
+      std::bind(&DummyActionServer::handle_goal, this, _1, _2),
+      std::bind(&DummyActionServer::handle_cancel, this, _1),
+      std::bind(&DummyActionServer::handle_accepted, this, _1));
+  }
+
+  void setFailureRanges(const std::vector<std::pair<int, int>> & failureRanges)
+  {
+    failure_ranges_ = failureRanges;
+  }
+
+  void setRunningRanges(const std::vector<std::pair<int, int>> & runningRanges)
+  {
+    running_ranges_ = runningRanges;
+  }
+
+  void reset()
+  {
+    failure_ranges_.clear();
+    running_ranges_.clear();
+    goal_count_ = 0;
+  }
+
+  int getGoalCount() const
+  {
+    return goal_count_;
+  }
+
+protected:
+  virtual std::shared_ptr<typename ActionT::Result> fillResult()
+  {
+    return std::make_shared<typename ActionT::Result>();
+  }
+
+  virtual rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID &,
+    std::shared_ptr<const typename ActionT::Goal>/*goal*/)
+  {
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  virtual rclcpp_action::CancelResponse handle_cancel(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)
+  {
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void execute(
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)
+  {
+    goal_count_++;
+    auto result = fillResult();
+
+    // if current goal index exists in running range, the thread sleeps for 1 second
+    // to simulate a long running action
+    for (auto & index : running_ranges_) {
+      if (goal_count_ >= index.first && goal_count_ <= index.second) {
+        std::this_thread::sleep_for(1s);
+        break;
+      }
+    }
+
+    // if current goal index exists in failure range, the goal will be aborted
+    for (auto & index : failure_ranges_) {
+      if (goal_count_ >= index.first && goal_count_ <= index.second) {
+        goal_handle->abort(result);
+        return;
+      }
+    }
+
+    // goal succeeds for all other indices
+    goal_handle->succeed(result);
+  }
+
+  void handle_accepted(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)
+  {
+    using namespace std::placeholders;  // NOLINT
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    std::thread{std::bind(&DummyActionServer::execute, this, _1), goal_handle}.detach();
+  }
+
+protected:
+  typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
+  std::string action_name_;
+
+  // contains pairs of indices which define a range for which the
+  // requested action goal will return running for 1s or be aborted
+  // for all other indices, the action server will return success
+  std::vector<std::pair<int, int>> failure_ranges_;
+  std::vector<std::pair<int, int>> running_ranges_;
+
+  int goal_count_;
+};
+
+#endif  // BEHAVIOR_TREE__DUMMY_SERVERS_HPP_

navigation2/nav2_system_tests/src/behavior_tree/server_handler.cpp

@@ -0,0 +1,96 @@
+// Copyright (c) 2020 Vinny Ruia
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#include <memory>
+#include <thread>
+
+#include "server_handler.hpp"
+
+using namespace std::chrono_literals;  // NOLINT
+using namespace std::chrono;  // NOLINT
+
+ServerHandler::ServerHandler()
+: is_active_(false)
+{
+  node_ = rclcpp::Node::make_shared("behavior_tree_tester");
+
+  clear_local_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(
+    node_, "local_costmap/clear_entirely_local_costmap");
+  clear_global_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(
+    node_, "global_costmap/clear_entirely_global_costmap");
+  compute_path_to_pose_server = std::make_unique<ComputePathToPoseActionServer>(node_);
+  follow_path_server = std::make_unique<DummyActionServer<nav2_msgs::action::FollowPath>>(
+    node_, "follow_path");
+  spin_server = std::make_unique<DummyActionServer<nav2_msgs::action::Spin>>(
+    node_, "spin");
+  wait_server = std::make_unique<DummyActionServer<nav2_msgs::action::Wait>>(
+    node_, "wait");
+  backup_server = std::make_unique<DummyActionServer<nav2_msgs::action::BackUp>>(
+    node_, "backup");
+  drive_on_heading_server = std::make_unique<DummyActionServer<nav2_msgs::action::DriveOnHeading>>(
+    node_, "drive_on_heading");
+  ntp_server = std::make_unique<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>>(
+    node_, "compute_path_through_poses");
+}
+
+ServerHandler::~ServerHandler()
+{
+  if (is_active_) {
+    deactivate();
+  }
+}
+
+void ServerHandler::activate()
+{
+  if (is_active_) {
+    throw std::runtime_error("Trying to activate while already activated");
+  }
+
+  is_active_ = true;
+  server_thread_ =
+    std::make_shared<std::thread>(std::bind(&ServerHandler::spinThread, this));
+
+  std::cout << "Server handler is active!" << std::endl;
+}
+
+void ServerHandler::deactivate()
+{
+  if (!is_active_) {
+    throw std::runtime_error("Trying to deactivate while already inactive");
+  }
+
+  is_active_ = false;
+  server_thread_->join();
+
+  std::cout << "Server handler has been deactivated!" << std::endl;
+}
+
+void ServerHandler::reset() const
+{
+  clear_global_costmap_server->reset();
+  clear_local_costmap_server->reset();
+  compute_path_to_pose_server->reset();
+  follow_path_server->reset();
+  spin_server->reset();
+  wait_server->reset();
+  backup_server->reset();
+  drive_on_heading_server->reset();
+}
+
+void ServerHandler::spinThread()
+{
+  rclcpp::spin(node_);
+}

navigation2/nav2_system_tests/src/behavior_tree/server_handler.hpp

@@ -0,0 +1,108 @@
+// Copyright (c) 2020 Vinny Ruia
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#ifndef BEHAVIOR_TREE__SERVER_HANDLER_HPP_
+#define BEHAVIOR_TREE__SERVER_HANDLER_HPP_
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include "nav2_msgs/srv/clear_entire_costmap.hpp"
+#include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_msgs/action/spin.hpp"
+#include "nav2_msgs/action/back_up.hpp"
+#include "nav2_msgs/action/wait.hpp"
+#include "nav2_msgs/action/drive_on_heading.hpp"
+#include "nav2_msgs/action/compute_path_through_poses.hpp"
+
+#include "geometry_msgs/msg/point_stamped.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "dummy_servers.hpp"
+
+class ComputePathToPoseActionServer
+  : public DummyActionServer<nav2_msgs::action::ComputePathToPose>
+{
+public:
+  explicit ComputePathToPoseActionServer(const rclcpp::Node::SharedPtr & node)
+  : DummyActionServer(node, "compute_path_to_pose")
+  {
+    result_ = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header = result_->path.header;
+    pose.pose.position.x = 0.0;
+    pose.pose.position.y = 0.0;
+    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;
+    for (int i = 0; i < 6; ++i) {
+      result_->path.poses.push_back(pose);
+    }
+  }
+
+  std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> fillResult() override
+  {
+    return result_;
+  }
+
+private:
+  std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> result_;
+};
+
+class ServerHandler
+{
+public:
+  ServerHandler();
+  ~ServerHandler();
+
+  void activate();
+
+  void deactivate();
+
+  bool isActive() const
+  {
+    return is_active_;
+  }
+
+  void reset() const;
+
+public:
+  std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_local_costmap_server;
+  std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_global_costmap_server;
+  std::unique_ptr<ComputePathToPoseActionServer> compute_path_to_pose_server;
+  std::unique_ptr<DummyActionServer<nav2_msgs::action::FollowPath>> follow_path_server;
+  std::unique_ptr<DummyActionServer<nav2_msgs::action::Spin>> spin_server;
+  std::unique_ptr<DummyActionServer<nav2_msgs::action::Wait>> wait_server;
+  std::unique_ptr<DummyActionServer<nav2_msgs::action::BackUp>> backup_server;
+  std::unique_ptr<DummyActionServer<nav2_msgs::action::DriveOnHeading>> drive_on_heading_server;
+  std::unique_ptr<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>> ntp_server;
+
+private:
+  void spinThread();
+
+  bool is_active_;
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<std::thread> server_thread_;
+};
+
+#endif  //  BEHAVIOR_TREE__SERVER_HANDLER_HPP_

navigation2/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp

@@ -0,0 +1,656 @@
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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. Reserved.
+
+#include <vector>
+#include <string>
+#include <fstream>
+#include <memory>
+#include <utility>
+#include <boost/filesystem.hpp>
+
+#include "gtest/gtest.h"
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/create_timer_ros.h"
+
+#include "nav2_util/odometry_utils.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
+#include "server_handler.hpp"
+
+using namespace std::chrono_literals;
+namespace fs = boost::filesystem;
+
+class BehaviorTreeHandler
+{
+public:
+  BehaviorTreeHandler()
+  {
+    node_ = rclcpp::Node::make_shared("behavior_tree_handler");
+
+    tf_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      node_->get_node_base_interface(), node_->get_node_timers_interface());
+    tf_->setCreateTimerInterface(timer_interface);
+    tf_->setUsingDedicatedThread(true);
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_, node_, false);
+
+    odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node_);
+
+    const std::vector<std::string> plugin_libs = {
+      "nav2_compute_path_to_pose_action_bt_node",
+      "nav2_compute_path_through_poses_action_bt_node",
+      "nav2_smooth_path_action_bt_node",
+      "nav2_follow_path_action_bt_node",
+      "nav2_spin_action_bt_node",
+      "nav2_wait_action_bt_node",
+      "nav2_assisted_teleop_action_bt_node",
+      "nav2_back_up_action_bt_node",
+      "nav2_drive_on_heading_bt_node",
+      "nav2_clear_costmap_service_bt_node",
+      "nav2_is_stuck_condition_bt_node",
+      "nav2_goal_reached_condition_bt_node",
+      "nav2_initial_pose_received_condition_bt_node",
+      "nav2_goal_updated_condition_bt_node",
+      "nav2_globally_updated_goal_condition_bt_node",
+      "nav2_is_path_valid_condition_bt_node",
+      "nav2_reinitialize_global_localization_service_bt_node",
+      "nav2_rate_controller_bt_node",
+      "nav2_distance_controller_bt_node",
+      "nav2_speed_controller_bt_node",
+      "nav2_truncate_path_action_bt_node",
+      "nav2_truncate_path_local_action_bt_node",
+      "nav2_goal_updater_node_bt_node",
+      "nav2_recovery_node_bt_node",
+      "nav2_pipeline_sequence_bt_node",
+      "nav2_round_robin_node_bt_node",
+      "nav2_transform_available_condition_bt_node",
+      "nav2_time_expired_condition_bt_node",
+      "nav2_path_expiring_timer_condition",
+      "nav2_distance_traveled_condition_bt_node",
+      "nav2_single_trigger_bt_node",
+      "nav2_is_battery_low_condition_bt_node",
+      "nav2_navigate_through_poses_action_bt_node",
+      "nav2_navigate_to_pose_action_bt_node",
+      "nav2_remove_passed_goals_action_bt_node",
+      "nav2_planner_selector_bt_node",
+      "nav2_controller_selector_bt_node",
+      "nav2_goal_checker_selector_bt_node",
+      "nav2_controller_cancel_bt_node",
+      "nav2_path_longer_on_approach_bt_node",
+      "nav2_assisted_teleop_cancel_bt_node",
+      "nav2_wait_cancel_bt_node",
+      "nav2_spin_cancel_bt_node",
+      "nav2_back_up_cancel_bt_node",
+      "nav2_drive_on_heading_cancel_bt_node",
+      "nav2_goal_updated_controller_bt_node"
+    };
+    for (const auto & p : plugin_libs) {
+      factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p));
+    }
+  }
+
+  bool loadBehaviorTree(const std::string & filename)
+  {
+    // Read the input BT XML from the specified file into a string
+    std::ifstream xml_file(filename);
+
+    if (!xml_file.good()) {
+      RCLCPP_ERROR(node_->get_logger(), "Couldn't open input XML file: %s", filename.c_str());
+      return false;
+    }
+
+    auto xml_string = std::string(
+      std::istreambuf_iterator<char>(xml_file),
+      std::istreambuf_iterator<char>());
+
+    // 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<rclcpp::Node::SharedPtr>("node", node_);  // NOLINT
+    blackboard->set<std::chrono::milliseconds>(
+      "server_timeout", std::chrono::milliseconds(20));  // NOLINT
+    blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration", std::chrono::milliseconds(10));  // NOLINT
+    blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout", std::chrono::milliseconds(1000));  // NOLINT
+    blackboard->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_);  // NOLINT
+    blackboard->set<bool>("initial_pose_received", false);  // NOLINT
+    blackboard->set<int>("number_recoveries", 0);  // NOLINT
+    blackboard->set<std::shared_ptr<nav2_util::OdomSmoother>>("odom_smoother", odom_smoother_);  // NOLINT
+
+    // set dummy goal on blackboard
+    geometry_msgs::msg::PoseStamped goal;
+    goal.header.stamp = node_->now();
+    goal.header.frame_id = "map";
+    goal.pose.position.x = 0.0;
+    goal.pose.position.y = 0.0;
+    goal.pose.position.z = 0.0;
+    goal.pose.orientation.x = 0.0;
+    goal.pose.orientation.y = 0.0;
+    goal.pose.orientation.z = 0.0;
+    goal.pose.orientation.w = 1.0;
+
+    blackboard->set<geometry_msgs::msg::PoseStamped>("goal", goal);  // NOLINT
+
+    // Create the Behavior Tree from the XML input
+    try {
+      tree = factory_.createTreeFromText(xml_string, blackboard);
+    } catch (BT::RuntimeError & exp) {
+      RCLCPP_ERROR(node_->get_logger(), "%s: %s", filename.c_str(), exp.what());
+      return false;
+    }
+
+    return true;
+  }
+
+public:
+  BT::Blackboard::Ptr blackboard;
+  BT::Tree tree;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
+
+  BT::BehaviorTreeFactory factory_;
+};
+
+class BehaviorTreeTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    // initialize ROS
+    rclcpp::init(0, nullptr);
+
+    server_handler = std::make_shared<ServerHandler>();
+    if (!server_handler->isActive()) {
+      server_handler->activate();
+    }
+  }
+
+  static void TearDownTestCase()
+  {
+    // shutdown ROS
+    rclcpp::shutdown();
+
+    server_handler.reset();
+    bt_handler.reset();
+  }
+
+  void SetUp() override
+  {
+    server_handler->reset();
+    bt_handler = std::make_shared<BehaviorTreeHandler>();
+  }
+
+  void TearDown() override
+  {
+    bt_handler.reset();
+  }
+
+protected:
+  static std::shared_ptr<ServerHandler> server_handler;
+  static std::shared_ptr<BehaviorTreeHandler> bt_handler;
+};
+
+std::shared_ptr<ServerHandler> BehaviorTreeTestFixture::server_handler = nullptr;
+std::shared_ptr<BehaviorTreeHandler> BehaviorTreeTestFixture::bt_handler = nullptr;
+
+TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles)
+{
+  fs::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  root /= "behavior_trees/";
+
+  if (boost::filesystem::exists(root) && boost::filesystem::is_directory(root)) {
+    for (auto const & entry : boost::filesystem::recursive_directory_iterator(root)) {
+      if (boost::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") {
+        std::cout << entry.path().string() << std::endl;
+        EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true);
+      }
+    }
+  }
+}
+
+/**
+ * Test scenario:
+ *
+ * ComputePathToPose and FollowPath return SUCCESS
+ * The behavior tree should execute correctly and return SUCCESS
+ */
+TEST_F(BehaviorTreeTestFixture, TestAllSuccess)
+{
+  // Load behavior tree from file
+  fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  bt_file /= "behavior_trees/";
+  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  while (result == BT::NodeStatus::RUNNING) {
+    result = bt_handler->tree.tickRoot();
+    std::this_thread::sleep_for(10ms);
+  }
+
+  // The final result should be success since all action servers returned success
+  EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
+
+  // Goal count should be 1 since only one goal is sent to ComputePathToPose and FollowPath servers
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 1);
+  EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 1);
+
+  // Goal count should be 0 since no goal is sent to all other servers
+  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 0);
+  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 0);
+}
+
+/**
+ * Test scenario:
+ *
+ * ComputePathToPose returns FAILURE and ClearGlobalCostmap-Context returns FAILURE
+ * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
+ * GoalUpdated returns FAILURE and RoundRobin is triggered
+ * RoundRobin triggers ClearingActions Sequence which returns FAILURE
+ * RoundRobin triggers Spin, Wait, and BackUp which return FAILURE
+ * RoundRobin returns FAILURE hence RecoveryCallbackk returns FAILURE
+ * Finally NavigateRecovery returns FAILURE
+ * The behavior tree should also return FAILURE
+ */
+TEST_F(BehaviorTreeTestFixture, TestAllFailure)
+{
+  // Load behavior tree from file
+  fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  bt_file /= "behavior_trees/";
+  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+
+  // Set all action server to fail the first 100 times
+  std::vector<std::pair<int, int>> failureRange;
+  failureRange.emplace_back(std::pair<int, int>(0, 100));
+  server_handler->compute_path_to_pose_server->setFailureRanges(failureRange);
+  server_handler->follow_path_server->setFailureRanges(failureRange);
+  server_handler->spin_server->setFailureRanges(failureRange);
+  server_handler->wait_server->setFailureRanges(failureRange);
+  server_handler->backup_server->setFailureRanges(failureRange);
+
+  // Disable services
+  server_handler->clear_global_costmap_server->disable();
+  server_handler->clear_local_costmap_server->disable();
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  while (result == BT::NodeStatus::RUNNING) {
+    result = bt_handler->tree.tickRoot();
+    std::this_thread::sleep_for(10ms);
+  }
+
+  // The final result should be failure
+  EXPECT_EQ(result, BT::NodeStatus::FAILURE);
+
+  // Goal count should be 1 since only one goal is sent to ComputePathToPose
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 1);
+
+  // Goal count should be 0 since no goal is sent to FollowPath action server
+  EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 0);
+
+  // All recovery action servers were sent 1 goal
+  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 1);
+  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 1);
+  EXPECT_EQ(server_handler->backup_server->getGoalCount(), 1);
+
+  // Service count is 0 since the server was disabled
+  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 0);
+  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 0);
+}
+
+/**
+ * Test scenario:
+ *
+ * ComputePathToPose returns FAILURE on the first try triggering the planner recovery
+ * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried
+ * FollowPath returns FAILURE on the first try triggering the controller recovery
+ * ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS when retried
+ * The behavior tree should return SUCCESS
+ */
+TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries)
+{
+  // Load behavior tree from file
+  fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  bt_file /= "behavior_trees/";
+  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+
+  // Set ComputePathToPose and FollowPath action servers to fail for the first action
+  std::vector<std::pair<int, int>> failureRange;
+  failureRange.emplace_back(std::pair<int, int>(0, 1));
+  server_handler->compute_path_to_pose_server->setFailureRanges(failureRange);
+  server_handler->follow_path_server->setFailureRanges(failureRange);
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  while (result == BT::NodeStatus::RUNNING) {
+    result = bt_handler->tree.tickRoot();
+    std::this_thread::sleep_for(10ms);
+  }
+
+  // The final result should be success
+  EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
+
+  // Goal count should be 2 since only two goals were sent to ComputePathToPose and FollowPath
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 2);
+  EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 2);
+
+  // Navigate subtree recovery services are called once each
+  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 1);
+  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 1);
+
+  // Goal count should be 0 since no goal is sent to all other servers
+  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0);
+}
+
+/**
+ * Test scenario:
+ *
+ * ComputePathToPose returns FAILURE on the first try triggering the planner recovery
+ * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried
+ * FollowPath returns FAILURE on the first try triggering the controller recovery
+ * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
+ * FollowPath returns FAILURE again and PipelineSequence returns FAILURE
+ * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
+ * RoundRobin triggers ClearingActions Sequence which returns SUCCESS
+ * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
+ * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS
+ * FollowPath returns FAILURE on the third try triggering the controller recovery
+ * ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS on the fourth try
+ * The behavior tree should return SUCCESS
+ */
+TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple)
+{
+  // Load behavior tree from file
+  fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  bt_file /= "behavior_trees/";
+  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+
+  // Set ComputePathToPose action server to fail for the first action
+  std::vector<std::pair<int, int>> plannerFailureRange;
+  plannerFailureRange.emplace_back(std::pair<int, int>(0, 1));
+  server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange);
+
+  // Set FollowPath action server to fail for the first 3 actions
+  std::vector<std::pair<int, int>> controllerFailureRange;
+  controllerFailureRange.emplace_back(std::pair<int, int>(0, 3));
+  server_handler->follow_path_server->setFailureRanges(controllerFailureRange);
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  while (result == BT::NodeStatus::RUNNING) {
+    result = bt_handler->tree.tickRoot();
+    std::this_thread::sleep_for(10ms);
+  }
+
+  // The final result should be success
+  EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
+
+  // FollowPath is called 4 times
+  EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 4);
+
+  // ComputePathToPose is called 3 times
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 3);
+
+  // Local costmap is cleared 3 times
+  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 3);
+
+  // Global costmap is cleared 2 times
+  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 2);
+
+  // Goal count should be 0 since only no goal is sent to all other servers
+  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0);
+}
+
+/**
+ * Test scenario:
+ *
+ * ComputePathToPose returns FAILURE on the first try triggering the planner recovery
+ * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
+ * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
+ * GoalUpdated returns FAILURE, RoundRobin triggers ClearingActions Sequence which returns SUCCESS
+ * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
+ *
+ * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS (retry #1)
+ * FollowPath returns FAILURE on the first try triggering the controller recovery
+ * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
+ * FollowPath returns FAILURE again and PipelineSequence returns FAILURE
+ * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
+ * RoundRobin triggers Spin which returns FAILURE
+ * RoundRobin triggers Wait which returns SUCCESS
+ * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
+ *
+ * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #2)
+ * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
+ * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
+ * GoalUpdated returns FAILURE and RoundRobin triggers BackUp which returns FAILURE
+ * RoundRobin triggers ClearingActions Sequence which returns SUCCESS
+ * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
+ *
+ * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #3)
+ * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
+ * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
+ * GoalUpdated returns FAILURE and RoundRobin triggers Spin which returns SUCCESS
+ * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
+ *
+ * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #4)
+ * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
+ * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
+ * GoalUpdated returns FAILURE and RoundRobin triggers Wait which returns FAILURE
+ * RoundRobin triggers BackUp which returns SUCCESS
+ * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
+ *
+ * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS (retry #5)
+ * FollowPath returns FAILURE on the first try triggering the controller recovery
+ * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
+ * FollowPath returns FAILURE again and PipelineSequence returns FAILURE
+ * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
+ * RoundRobin triggers ClearingActions Sequence which returns SUCCESS
+ * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
+ *
+ * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #6)
+ * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
+ * PipelineSequence returns FAILURE and NavigateRecovery finally also returns FAILURE
+ *
+ * The behavior tree should return FAILURE
+ */
+TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex)
+{
+  // Load behavior tree from file
+  fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  bt_file /= "behavior_trees/";
+  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+
+  // Set ComputePathToPose action server to fail for the first 2 actions
+  std::vector<std::pair<int, int>> plannerFailureRange;
+  plannerFailureRange.emplace_back(std::pair<int, int>(0, 2));
+  plannerFailureRange.emplace_back(std::pair<int, int>(4, 9));
+  plannerFailureRange.emplace_back(std::pair<int, int>(11, 12));
+  server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange);
+
+  // Set FollowPath action server to fail for the first 2 actions
+  std::vector<std::pair<int, int>> controllerFailureRange;
+  controllerFailureRange.emplace_back(std::pair<int, int>(0, 4));
+  server_handler->follow_path_server->setFailureRanges(controllerFailureRange);
+
+  // Set Spin action server to fail for the first action
+  std::vector<std::pair<int, int>> spinFailureRange;
+  spinFailureRange.emplace_back(std::pair<int, int>(0, 1));
+  server_handler->spin_server->setFailureRanges(spinFailureRange);
+
+  // Set Wait action server to fail for the first action
+  std::vector<std::pair<int, int>> waitFailureRange;
+  waitFailureRange.emplace_back(std::pair<int, int>(2, 2));
+  server_handler->wait_server->setFailureRanges(waitFailureRange);
+
+  // Set BackUp action server to fail for the first action
+  std::vector<std::pair<int, int>> backupFailureRange;
+  backupFailureRange.emplace_back(std::pair<int, int>(0, 1));
+  server_handler->backup_server->setFailureRanges(backupFailureRange);
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  while (result == BT::NodeStatus::RUNNING) {
+    result = bt_handler->tree.tickRoot();
+    std::this_thread::sleep_for(10ms);
+  }
+
+  // The final result should be success
+  EXPECT_EQ(result, BT::NodeStatus::FAILURE);
+
+  // ComputePathToPose is called 12 times
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 12);
+
+  // FollowPath is called 4 times
+  EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 4);
+
+  // Local costmap is cleared 5 times
+  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 5);
+
+  // Global costmap is cleared 8 times
+  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 8);
+
+  // All recovery action servers receive 2 goals
+  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 2);
+  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 2);
+  EXPECT_EQ(server_handler->backup_server->getGoalCount(), 2);
+}
+
+/**
+ * Test scenario:
+ *
+ * ComputePathToPose returns FAILURE on the first try triggering the planner recovery
+ * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
+ * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
+ * GoalUpdated returns FAILURE, RoundRobin triggers ClearingActions Sequence which returns SUCCESS
+ * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
+ * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS
+ * FollowPath returns FAILURE on the first try triggering the controller recovery
+ * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
+ * FollowPath returns FAILURE and PipelineSequence returns FAILURE
+ * NavigateRecovery triggers RecoveryFallback which triggers GoalUpdated
+ * GoalUpdated returns FAILURE and RecoveryFallback triggers RoundRobin
+ * RoundRobin triggers Spin which returns RUNNING
+ *
+ * At this point a new goal is updated on the blackboard
+ *
+ * RecoveryFallback triggers GoalUpdated which returns SUCCESS this time
+ * Since GoalUpdated returned SUCCESS, RoundRobin and hence Spin is halted
+ * RecoveryFallback also returns SUCCESS and PipelineSequence is retried
+ * PipelineSequence triggers ComputePathToPose which returns SUCCESS
+ * FollowPath returns SUCCESS and NavigateRecovery finally also returns SUCCESS
+ *
+ * The behavior tree should return SUCCESS
+ */
+TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated)
+{
+  // Load behavior tree from file
+  fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  bt_file /= "behavior_trees/";
+  bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
+  EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true);
+
+  // Set ComputePathToPose action server to fail for the first 2 actions
+  std::vector<std::pair<int, int>> plannerFailureRange;
+  plannerFailureRange.emplace_back(std::pair<int, int>(0, 2));
+  server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange);
+
+  // Set FollowPath action server to fail for the first 2 actions
+  std::vector<std::pair<int, int>> controllerFailureRange;
+  controllerFailureRange.emplace_back(std::pair<int, int>(0, 2));
+  server_handler->follow_path_server->setFailureRanges(controllerFailureRange);
+
+  // Set Spin action server to return running for the first action
+  std::vector<std::pair<int, int>> spinRunningRange;
+  spinRunningRange.emplace_back(std::pair<int, int>(1, 1));
+  server_handler->spin_server->setRunningRanges(spinRunningRange);
+
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  while (result == BT::NodeStatus::RUNNING) {
+    result = bt_handler->tree.tickRoot();
+
+    // Update goal on blackboard after Spin has been triggered once
+    // to simulate a goal update during a recovery action
+    if (server_handler->spin_server->getGoalCount() > 0) {
+      geometry_msgs::msg::PoseStamped goal;
+      goal.pose.position.x = 1.0;
+      goal.pose.position.y = 1.0;
+      goal.pose.position.z = 1.0;
+      goal.pose.orientation.x = 0.0;
+      goal.pose.orientation.y = 0.0;
+      goal.pose.orientation.z = 0.0;
+      goal.pose.orientation.w = 1.0;
+      bt_handler->blackboard->set<geometry_msgs::msg::PoseStamped>("goal", goal);  // NOLINT
+    }
+
+    std::this_thread::sleep_for(10ms);
+  }
+
+  // The final result should be success
+  EXPECT_EQ(result, BT::NodeStatus::SUCCESS);
+
+  // ComputePathToPose is called 4 times
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 4);
+
+  // FollowPath is called 3 times
+  EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 3);
+
+  // Local costmap is cleared 2 times
+  EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 2);
+
+  // Global costmap is cleared 2 times
+  EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 2);
+
+  // Spin server receives 1 action
+  EXPECT_EQ(server_handler->spin_server->getGoalCount(), 1);
+
+  // All recovery action servers receive 0 goals
+  EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  bool all_successful = RUN_ALL_TESTS();
+  return all_successful;
+}

navigation2/nav2_system_tests/src/behaviors/README.md

@@ -0,0 +1,4 @@
+# Behavior Test
+
+Provides some simple tests for behavior plugins.
+It creates an instance of the stack, with the behavior server loading different behavior plugins, and checks for successful behavior behaviors.

navigation2/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt

@@ -0,0 +1,23 @@
+set(test_assisted_teleop_behavior test_assisted_teleop_behavior_node)
+
+ament_add_gtest_executable(${test_assisted_teleop_behavior}
+  test_assisted_teleop_behavior_node.cpp
+  assisted_teleop_behavior_tester.cpp
+)
+
+ament_target_dependencies(${test_assisted_teleop_behavior}
+  ${dependencies}
+)
+
+ament_add_test(test_assisted_teleop_behavior
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_assisted_teleop_behavior_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_assisted_teleop_behavior}>
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+)

navigation2/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp

@@ -0,0 +1,277 @@
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#include <string>
+#include <random>
+#include <tuple>
+#include <memory>
+#include <iostream>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+
+#include "assisted_teleop_behavior_tester.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_system_tests
+{
+
+AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester()
+: is_active_(false),
+  initial_pose_received_(false)
+{
+  node_ = rclcpp::Node::make_shared("assisted_teleop_behavior_test");
+
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  client_ptr_ = rclcpp_action::create_client<AssistedTeleop>(
+    node_->get_node_base_interface(),
+    node_->get_node_graph_interface(),
+    node_->get_node_logging_interface(),
+    node_->get_node_waitables_interface(),
+    "assisted_teleop");
+
+  initial_pose_pub_ =
+    node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
+
+  preempt_pub_ =
+    node_->create_publisher<std_msgs::msg::Empty>("preempt_teleop", 10);
+
+  cmd_vel_pub_ =
+    node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel_teleop", 10);
+
+  subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
+
+  filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
+    "cmd_vel",
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1));
+
+  std::string costmap_topic = "/local_costmap/costmap_raw";
+  std::string footprint_topic = "/local_costmap/published_footprint";
+
+  costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
+    node_,
+    costmap_topic);
+
+  footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
+    node_,
+    footprint_topic,
+    *tf_buffer_);
+
+  collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(
+    *costmap_sub_,
+    *footprint_sub_
+  );
+
+  stamp_ = node_->now();
+}
+
+AssistedTeleopBehaviorTester::~AssistedTeleopBehaviorTester()
+{
+  if (is_active_) {
+    deactivate();
+  }
+}
+
+void AssistedTeleopBehaviorTester::activate()
+{
+  if (is_active_) {
+    throw std::runtime_error("Trying to activate while already active");
+    return;
+  }
+
+  while (!initial_pose_received_) {
+    RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
+    sendInitialPose();
+    std::this_thread::sleep_for(100ms);
+    rclcpp::spin_some(node_);
+  }
+
+  // Wait for lifecycle_manager_navigation to activate behavior_server
+  std::this_thread::sleep_for(10s);
+
+  if (!client_ptr_) {
+    RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
+    is_active_ = false;
+    return;
+  }
+
+  if (!client_ptr_->wait_for_action_server(10s)) {
+    RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
+    is_active_ = false;
+    return;
+  }
+
+  RCLCPP_INFO(this->node_->get_logger(), "Assisted Teleop action server is ready");
+  is_active_ = true;
+}
+
+void AssistedTeleopBehaviorTester::deactivate()
+{
+  if (!is_active_) {
+    throw std::runtime_error("Trying to deactivate while already inactive");
+  }
+  is_active_ = false;
+}
+
+bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest(
+  const float lin_vel,
+  const float ang_vel)
+{
+  if (!is_active_) {
+    RCLCPP_ERROR(node_->get_logger(), "Not activated");
+    return false;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "Sending goal");
+
+  auto goal_handle_future = client_ptr_->async_send_goal(nav2_msgs::action::AssistedTeleop::Goal());
+
+  if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<AssistedTeleop>::SharedPtr goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
+    return false;
+  }
+
+  // Wait for the server to be done with the goal
+  auto result_future = client_ptr_->async_get_result(goal_handle);
+
+  rclcpp::Rate r(1);
+
+  counter_ = 0;
+  auto start_time = std::chrono::system_clock::now();
+  while (rclcpp::ok()) {
+    geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist();
+    cmd_vel.linear.x = lin_vel;
+    cmd_vel.angular.z = ang_vel;
+    cmd_vel_pub_->publish(cmd_vel);
+
+    if (counter_ > 1) {
+      break;
+    }
+
+    auto current_time = std::chrono::system_clock::now();
+    if (current_time - start_time > 25s) {
+      RCLCPP_ERROR(node_->get_logger(), "Exceeded Timeout");
+      return false;
+    }
+
+    rclcpp::spin_some(node_);
+    r.sleep();
+  }
+
+  auto preempt_msg = std_msgs::msg::Empty();
+  preempt_pub_->publish(preempt_msg);
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for result");
+  if (rclcpp::spin_until_future_complete(node_, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<AssistedTeleop>::WrappedResult
+    wrapped_result = result_future.get();
+
+  switch (wrapped_result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED: break;
+    case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was aborted");
+      return false;
+    case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was canceled");
+      return false;
+    default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
+      return false;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "result received");
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) {
+    RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
+    return false;
+  }
+
+  geometry_msgs::msg::Pose2D pose_2d;
+  pose_2d.x = current_pose.pose.position.x;
+  pose_2d.y = current_pose.pose.position.y;
+  pose_2d.theta = tf2::getYaw(current_pose.pose.orientation);
+
+  if (!collision_checker_->isCollisionFree(pose_2d)) {
+    RCLCPP_ERROR(node_->get_logger(), "Ended in collision");
+    return false;
+  }
+
+  return true;
+}
+
+void AssistedTeleopBehaviorTester::sendInitialPose()
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped pose;
+  pose.header.frame_id = "map";
+  pose.header.stamp = stamp_;
+  pose.pose.pose.position.x = -2.0;
+  pose.pose.pose.position.y = -0.5;
+  pose.pose.pose.position.z = 0.0;
+  pose.pose.pose.orientation.x = 0.0;
+  pose.pose.pose.orientation.y = 0.0;
+  pose.pose.pose.orientation.z = 0.0;
+  pose.pose.pose.orientation.w = 1.0;
+  for (int i = 0; i < 35; i++) {
+    pose.pose.covariance[i] = 0.0;
+  }
+  pose.pose.covariance[0] = 0.08;
+  pose.pose.covariance[7] = 0.08;
+  pose.pose.covariance[35] = 0.05;
+
+  initial_pose_pub_->publish(pose);
+  RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
+}
+
+void AssistedTeleopBehaviorTester::amclPoseCallback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
+{
+  initial_pose_received_ = true;
+}
+
+void AssistedTeleopBehaviorTester::filteredVelCallback(
+  geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  if (msg->linear.x == 0.0f) {
+    counter_++;
+  } else {
+    counter_ = 0;
+  }
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp

@@ -0,0 +1,104 @@
+// Copyright (c) 2020 Samsung Research
+// 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. Reserved.
+
+#ifndef BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_
+#define BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <thread>
+#include <algorithm>
+
+#include "angles/angles.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
+#include "nav2_msgs/action/assisted_teleop.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/empty.hpp"
+
+#include "tf2/utils.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+namespace nav2_system_tests
+{
+
+class AssistedTeleopBehaviorTester
+{
+public:
+  using AssistedTeleop = nav2_msgs::action::AssistedTeleop;
+
+  AssistedTeleopBehaviorTester();
+  ~AssistedTeleopBehaviorTester();
+
+  // Runs a single test with given target yaw
+  bool defaultAssistedTeleopTest(
+    const float lin_vel,
+    const float ang_vel);
+
+  void activate();
+
+  void deactivate();
+
+  bool isActive() const
+  {
+    return is_active_;
+  }
+
+private:
+  void sendInitialPose();
+
+  void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
+
+  void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg);
+
+  unsigned int counter_;
+  bool is_active_;
+  bool initial_pose_received_;
+  rclcpp::Time stamp_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  // Publishers
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
+  rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr preempt_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+
+  // Subscribers
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr filtered_vel_sub_;
+
+  // Action client to call AssistedTeleop action
+  rclcpp_action::Client<AssistedTeleop>::SharedPtr client_ptr_;
+
+  // collision checking
+  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  std::unique_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py

@@ -0,0 +1,103 @@
+#! /usr/bin/env python3
+# Copyright (c) 2012 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites='',
+        convert_types=True)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': configured_params,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_assisted_teleop_behavior_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp

@@ -0,0 +1,114 @@
+// Copyright (c) 2020 Samsung Research
+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <tuple>
+#include <string>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "assisted_teleop_behavior_tester.hpp"
+#include "nav2_msgs/action/back_up.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::AssistedTeleopBehaviorTester;
+
+struct TestParameters
+{
+  float lin_vel;
+  float ang_vel;
+};
+
+
+std::string testNameGenerator(const testing::TestParamInfo<TestParameters> &)
+{
+  static int test_index = 0;
+  std::string name = "AssistedTeleopTest" + std::to_string(test_index);
+  ++test_index;
+  return name;
+}
+
+class AssistedTeleopBehaviorTestFixture
+  : public ::testing::TestWithParam<TestParameters>
+{
+public:
+  static void SetUpTestCase()
+  {
+    assisted_teleop_behavior_tester = new AssistedTeleopBehaviorTester();
+    if (!assisted_teleop_behavior_tester->isActive()) {
+      assisted_teleop_behavior_tester->activate();
+    }
+  }
+
+  static void TearDownTestCase()
+  {
+    delete assisted_teleop_behavior_tester;
+    assisted_teleop_behavior_tester = nullptr;
+  }
+
+protected:
+  static AssistedTeleopBehaviorTester * assisted_teleop_behavior_tester;
+};
+
+AssistedTeleopBehaviorTester *
+AssistedTeleopBehaviorTestFixture::assisted_teleop_behavior_tester = nullptr;
+
+TEST_P(AssistedTeleopBehaviorTestFixture, testAssistedTeleopBehavior)
+{
+  auto test_params = GetParam();
+
+  if (!assisted_teleop_behavior_tester->isActive()) {
+    assisted_teleop_behavior_tester->activate();
+  }
+
+  bool success = false;
+  success = assisted_teleop_behavior_tester->defaultAssistedTeleopTest(
+    test_params.lin_vel,
+    test_params.ang_vel);
+
+  EXPECT_TRUE(success);
+}
+
+std::vector<TestParameters> test_params = {TestParameters{-0.1, 0.0},
+  TestParameters{0.35, 0.05}};
+
+INSTANTIATE_TEST_SUITE_P(
+  TestAssistedTeleopBehavior,
+  AssistedTeleopBehaviorTestFixture,
+  ::testing::Values(
+    test_params[0],
+    test_params[1]),
+  testNameGenerator
+);
+
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_system_tests/src/behaviors/backup/CMakeLists.txt

@@ -0,0 +1,23 @@
+set(test_backup_behavior test_backup_behavior_node)
+
+ament_add_gtest_executable(${test_backup_behavior}
+  test_backup_behavior_node.cpp
+  backup_behavior_tester.cpp
+)
+
+ament_target_dependencies(${test_backup_behavior}
+  ${dependencies}
+)
+
+ament_add_test(test_backup_recovery
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_behavior_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_backup_behavior}>
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+)

navigation2/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp

@@ -0,0 +1,223 @@
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#include <string>
+#include <random>
+#include <tuple>
+#include <memory>
+#include <iostream>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+
+#include "backup_behavior_tester.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_system_tests
+{
+
+BackupBehaviorTester::BackupBehaviorTester()
+: is_active_(false),
+  initial_pose_received_(false)
+{
+  rclcpp::NodeOptions options;
+  options.parameter_overrides({{"use_sim_time", true}});
+  node_ = rclcpp::Node::make_shared("backup_behavior_test", options);
+
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  client_ptr_ = rclcpp_action::create_client<BackUp>(
+    node_->get_node_base_interface(),
+    node_->get_node_graph_interface(),
+    node_->get_node_logging_interface(),
+    node_->get_node_waitables_interface(),
+    "backup");
+
+  publisher_ =
+    node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
+
+  subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&BackupBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
+
+  stamp_ = node_->now();
+}
+
+BackupBehaviorTester::~BackupBehaviorTester()
+{
+  if (is_active_) {
+    deactivate();
+  }
+}
+
+void BackupBehaviorTester::activate()
+{
+  if (is_active_) {
+    throw std::runtime_error("Trying to activate while already active");
+    return;
+  }
+
+  while (!initial_pose_received_) {
+    RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
+    sendInitialPose();
+    std::this_thread::sleep_for(100ms);
+    rclcpp::spin_some(node_);
+  }
+
+  // Wait for lifecycle_manager_navigation to activate behavior_server
+  std::this_thread::sleep_for(10s);
+
+  if (!client_ptr_) {
+    RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
+    is_active_ = false;
+    return;
+  }
+
+  if (!client_ptr_->wait_for_action_server(10s)) {
+    RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
+    is_active_ = false;
+    return;
+  }
+
+  RCLCPP_INFO(this->node_->get_logger(), "Backup action server is ready");
+  is_active_ = true;
+}
+
+void BackupBehaviorTester::deactivate()
+{
+  if (!is_active_) {
+    throw std::runtime_error("Trying to deactivate while already inactive");
+  }
+  is_active_ = false;
+}
+
+bool BackupBehaviorTester::defaultBackupBehaviorTest(
+  const BackUp::Goal goal_msg,
+  const double tolerance)
+{
+  if (!is_active_) {
+    RCLCPP_ERROR(node_->get_logger(), "Not activated");
+    return false;
+  }
+
+  // Sleep to let behavior server be ready for serving in multiple runs
+  std::this_thread::sleep_for(5s);
+
+  RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
+
+  geometry_msgs::msg::PoseStamped initial_pose;
+  if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) {
+    RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Found current robot pose");
+
+  auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
+
+  if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<BackUp>::SharedPtr goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
+    return false;
+  }
+
+  // Wait for the server to be done with the goal
+  auto result_future = client_ptr_->async_get_result(goal_handle);
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for result");
+  if (rclcpp::spin_until_future_complete(node_, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<BackUp>::WrappedResult wrapped_result = result_future.get();
+
+  switch (wrapped_result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED: break;
+    case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was aborted");
+      return false;
+    case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was canceled");
+      return false;
+    default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
+      return false;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "result received");
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) {
+    RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
+    return false;
+  }
+
+  double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose);
+
+  if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "Distance from goal is %lf (tolerance %lf)",
+      fabs(dist - goal_msg.target.x), tolerance);
+    return false;
+  }
+
+  return true;
+}
+
+void BackupBehaviorTester::sendInitialPose()
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped pose;
+  pose.header.frame_id = "map";
+  pose.header.stamp = stamp_;
+  pose.pose.pose.position.x = -2.0;
+  pose.pose.pose.position.y = -0.5;
+  pose.pose.pose.position.z = 0.0;
+  pose.pose.pose.orientation.x = 0.0;
+  pose.pose.pose.orientation.y = 0.0;
+  pose.pose.pose.orientation.z = 0.0;
+  pose.pose.pose.orientation.w = 1.0;
+  for (int i = 0; i < 35; i++) {
+    pose.pose.covariance[i] = 0.0;
+  }
+  pose.pose.covariance[0] = 0.08;
+  pose.pose.covariance[7] = 0.08;
+  pose.pose.covariance[35] = 0.05;
+
+  publisher_->publish(pose);
+  RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
+}
+
+void BackupBehaviorTester::amclPoseCallback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
+{
+  initial_pose_received_ = true;
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp

@@ -0,0 +1,90 @@
+// Copyright (c) 2020 Samsung Research
+// 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. Reserved.
+
+#ifndef BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
+#define BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <thread>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "angles/angles.h"
+#include "nav2_msgs/action/back_up.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+
+#include "tf2/utils.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+namespace nav2_system_tests
+{
+
+class BackupBehaviorTester
+{
+public:
+  using BackUp = nav2_msgs::action::BackUp;
+  using GoalHandleBackup = rclcpp_action::ClientGoalHandle<BackUp>;
+
+  BackupBehaviorTester();
+  ~BackupBehaviorTester();
+
+  // Runs a single test with given target yaw
+  bool defaultBackupBehaviorTest(
+    const BackUp::Goal goal_msg,
+    const double tolerance);
+
+  void activate();
+
+  void deactivate();
+
+  bool isActive() const
+  {
+    return is_active_;
+  }
+
+private:
+  void sendInitialPose();
+
+  void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
+
+  bool is_active_;
+  bool initial_pose_received_;
+  rclcpp::Time stamp_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  // Publisher to publish initial pose
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
+
+  // Subscriber for amcl pose
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
+
+  // Action client to call Backup action
+  rclcpp_action::Client<BackUp>::SharedPtr client_ptr_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py

@@ -0,0 +1,102 @@
+#! /usr/bin/env python3
+# Copyright (c) 2012 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites='',
+        convert_types=True)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': configured_params,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable], name='test_backup_behavior_node', output='screen',
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp

@@ -0,0 +1,128 @@
+// Copyright (c) 2020 Samsung Research
+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <tuple>
+#include <string>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "backup_behavior_tester.hpp"
+#include "nav2_msgs/action/back_up.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::BackupBehaviorTester;
+
+struct TestParameters
+{
+  float x;
+  float y;
+  float speed;
+  float tolerance;
+};
+
+
+std::string testNameGenerator(const testing::TestParamInfo<TestParameters> &)
+{
+  static int test_index = 0;
+  std::string name = "BackUpTest" + std::to_string(test_index);
+  ++test_index;
+  return name;
+}
+
+class BackupBehaviorTestFixture
+  : public ::testing::TestWithParam<TestParameters>
+{
+public:
+  static void SetUpTestCase()
+  {
+    backup_behavior_tester = new BackupBehaviorTester();
+    if (!backup_behavior_tester->isActive()) {
+      backup_behavior_tester->activate();
+    }
+  }
+
+  static void TearDownTestCase()
+  {
+    delete backup_behavior_tester;
+    backup_behavior_tester = nullptr;
+  }
+
+protected:
+  static BackupBehaviorTester * backup_behavior_tester;
+};
+
+BackupBehaviorTester * BackupBehaviorTestFixture::backup_behavior_tester = nullptr;
+
+TEST_P(BackupBehaviorTestFixture, testBackupBehavior)
+{
+  auto test_params = GetParam();
+  auto goal = nav2_msgs::action::BackUp::Goal();
+  goal.target.x = test_params.x;
+  goal.target.y = test_params.y;
+  goal.speed = test_params.speed;
+  float tolerance = test_params.tolerance;
+
+  if (!backup_behavior_tester->isActive()) {
+    backup_behavior_tester->activate();
+  }
+
+  bool success = false;
+  success = backup_behavior_tester->defaultBackupBehaviorTest(goal, tolerance);
+
+  float dist_to_obstacle = 2.0f;
+
+  if ( ((dist_to_obstacle - std::fabs(test_params.x)) < std::fabs(goal.speed)) ||
+    std::fabs(goal.target.y) > 0)
+  {
+    EXPECT_FALSE(success);
+  } else {
+    EXPECT_TRUE(success);
+  }
+}
+
+std::vector<TestParameters> test_params = {TestParameters{-0.05, 0.0, -0.2, 0.01},
+  TestParameters{-0.05, 0.1, -0.2, 0.01},
+  TestParameters{-2.0, 0.0, -0.2, 0.1}};
+
+INSTANTIATE_TEST_SUITE_P(
+  BackupBehaviorTests,
+  BackupBehaviorTestFixture,
+  ::testing::Values(
+    test_params[0],
+    test_params[1],
+    test_params[2]),
+  testNameGenerator
+);
+
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt

@@ -0,0 +1,23 @@
+set(test_drive_on_heading_behavior test_drive_on_heading_behavior_node)
+
+ament_add_gtest_executable(${test_drive_on_heading_behavior}
+  test_drive_on_heading_behavior_node.cpp
+  drive_on_heading_behavior_tester.cpp
+)
+
+ament_target_dependencies(${test_drive_on_heading_behavior}
+  ${dependencies}
+)
+
+ament_add_test(test_drive_on_heading_recovery
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_drive_on_heading_behavior_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_drive_on_heading_behavior}>
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+)

navigation2/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp

@@ -0,0 +1,224 @@
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#include <string>
+#include <random>
+#include <tuple>
+#include <memory>
+#include <iostream>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+
+#include "drive_on_heading_behavior_tester.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_system_tests
+{
+
+DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester()
+: is_active_(false),
+  initial_pose_received_(false)
+{
+  rclcpp::NodeOptions options;
+  options.parameter_overrides({{"use_sim_time", true}});
+  node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test", options);
+
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  client_ptr_ = rclcpp_action::create_client<DriveOnHeading>(
+    node_->get_node_base_interface(),
+    node_->get_node_graph_interface(),
+    node_->get_node_logging_interface(),
+    node_->get_node_waitables_interface(),
+    "drive_on_heading");
+
+  publisher_ =
+    node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
+
+  subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&DriveOnHeadingBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
+
+  stamp_ = node_->now();
+}
+
+DriveOnHeadingBehaviorTester::~DriveOnHeadingBehaviorTester()
+{
+  if (is_active_) {
+    deactivate();
+  }
+}
+
+void DriveOnHeadingBehaviorTester::activate()
+{
+  if (is_active_) {
+    throw std::runtime_error("Trying to activate while already active");
+    return;
+  }
+
+  while (!initial_pose_received_) {
+    RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
+    sendInitialPose();
+    std::this_thread::sleep_for(100ms);
+    rclcpp::spin_some(node_);
+  }
+
+  // Wait for lifecycle_manager_navigation to activate behavior_server
+  std::this_thread::sleep_for(10s);
+
+  if (!client_ptr_) {
+    RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
+    is_active_ = false;
+    return;
+  }
+
+  if (!client_ptr_->wait_for_action_server(10s)) {
+    RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
+    is_active_ = false;
+    return;
+  }
+
+  RCLCPP_INFO(this->node_->get_logger(), "DriveOnHeading action server is ready");
+  is_active_ = true;
+}
+
+void DriveOnHeadingBehaviorTester::deactivate()
+{
+  if (!is_active_) {
+    throw std::runtime_error("Trying to deactivate while already inactive");
+  }
+  is_active_ = false;
+}
+
+bool DriveOnHeadingBehaviorTester::defaultDriveOnHeadingBehaviorTest(
+  const DriveOnHeading::Goal goal_msg,
+  const double tolerance)
+{
+  if (!is_active_) {
+    RCLCPP_ERROR(node_->get_logger(), "Not activated");
+    return false;
+  }
+
+  // Sleep to let behavior server be ready for serving in multiple runs
+  std::this_thread::sleep_for(5s);
+
+  RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
+
+  geometry_msgs::msg::PoseStamped initial_pose;
+  if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) {
+    RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Found current robot pose");
+
+  auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
+
+  if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<DriveOnHeading>::SharedPtr goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
+    return false;
+  }
+
+  // Wait for the server to be done with the goal
+  auto result_future = client_ptr_->async_get_result(goal_handle);
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for result");
+  if (rclcpp::spin_until_future_complete(node_, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<DriveOnHeading>::WrappedResult wrapped_result =
+    result_future.get();
+
+  switch (wrapped_result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED: break;
+    case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was aborted");
+      return false;
+    case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was canceled");
+      return false;
+    default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
+      return false;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "result received");
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) {
+    RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
+    return false;
+  }
+
+  double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose);
+
+  if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "Distance from goal is %lf (tolerance %lf)",
+      fabs(dist - goal_msg.target.x), tolerance);
+    return false;
+  }
+
+  return true;
+}
+
+void DriveOnHeadingBehaviorTester::sendInitialPose()
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped pose;
+  pose.header.frame_id = "map";
+  pose.header.stamp = stamp_;
+  pose.pose.pose.position.x = -2.0;
+  pose.pose.pose.position.y = -0.5;
+  pose.pose.pose.position.z = 0.0;
+  pose.pose.pose.orientation.x = 0.0;
+  pose.pose.pose.orientation.y = 0.0;
+  pose.pose.pose.orientation.z = 0.0;
+  pose.pose.pose.orientation.w = 1.0;
+  for (int i = 0; i < 35; i++) {
+    pose.pose.covariance[i] = 0.0;
+  }
+  pose.pose.covariance[0] = 0.08;
+  pose.pose.covariance[7] = 0.08;
+  pose.pose.covariance[35] = 0.05;
+
+  publisher_->publish(pose);
+  RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
+}
+
+void DriveOnHeadingBehaviorTester::amclPoseCallback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
+{
+  initial_pose_received_ = true;
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp

@@ -0,0 +1,90 @@
+// Copyright (c) 2020 Samsung Research
+// 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. Reserved.
+
+#ifndef BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
+#define BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <thread>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "angles/angles.h"
+#include "nav2_msgs/action/drive_on_heading.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+
+#include "tf2/utils.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+namespace nav2_system_tests
+{
+
+class DriveOnHeadingBehaviorTester
+{
+public:
+  using DriveOnHeading = nav2_msgs::action::DriveOnHeading;
+  using GoalHandleDriveOnHeading = rclcpp_action::ClientGoalHandle<DriveOnHeading>;
+
+  DriveOnHeadingBehaviorTester();
+  ~DriveOnHeadingBehaviorTester();
+
+  // Runs a single test with given target yaw
+  bool defaultDriveOnHeadingBehaviorTest(
+    const DriveOnHeading::Goal goal_msg,
+    double tolerance);
+
+  void activate();
+
+  void deactivate();
+
+  bool isActive() const
+  {
+    return is_active_;
+  }
+
+private:
+  void sendInitialPose();
+
+  void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
+
+  bool is_active_;
+  bool initial_pose_received_;
+  rclcpp::Time stamp_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  // Publisher to publish initial pose
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
+
+  // Subscriber for amcl pose
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
+
+  // Action client to call DriveOnHeading action
+  rclcpp_action::Client<DriveOnHeading>::SharedPtr client_ptr_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py

@@ -0,0 +1,103 @@
+#! /usr/bin/env python3
+# Copyright (c) 2012 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites='',
+        convert_types=True)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': configured_params,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_drive_on_heading_behavior_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp

@@ -0,0 +1,133 @@
+// Copyright (c) 2020 Samsung Research
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <tuple>
+#include <string>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "drive_on_heading_behavior_tester.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::DriveOnHeadingBehaviorTester;
+
+struct TestParameters
+{
+  float x;
+  float y;
+  float speed;
+  float time_allowance;
+  float tolerance;
+};
+
+std::string testNameGenerator(const testing::TestParamInfo<TestParameters> &)
+{
+  static int test_index = 0;
+  std::string name = "DriveOnHeadingTest" + std::to_string(test_index);
+  ++test_index;
+  return name;
+}
+
+class DriveOnHeadingBehaviorTestFixture
+  : public ::testing::TestWithParam<TestParameters>
+{
+public:
+  static void SetUpTestCase()
+  {
+    drive_on_heading_behavior_tester = new DriveOnHeadingBehaviorTester();
+    if (!drive_on_heading_behavior_tester->isActive()) {
+      drive_on_heading_behavior_tester->activate();
+    }
+  }
+
+  static void TearDownTestCase()
+  {
+    delete drive_on_heading_behavior_tester;
+    drive_on_heading_behavior_tester = nullptr;
+  }
+
+protected:
+  static DriveOnHeadingBehaviorTester * drive_on_heading_behavior_tester;
+};
+
+DriveOnHeadingBehaviorTester * DriveOnHeadingBehaviorTestFixture::drive_on_heading_behavior_tester =
+  nullptr;
+
+TEST_P(DriveOnHeadingBehaviorTestFixture, testBackupBehavior)
+{
+  auto test_params = GetParam();
+  auto goal = nav2_msgs::action::DriveOnHeading::Goal();
+  goal.target.x = test_params.x;
+  goal.target.y = test_params.y;
+  goal.speed = test_params.speed;
+  goal.time_allowance.sec = test_params.time_allowance;
+  float tolerance = test_params.tolerance;
+
+  if (!drive_on_heading_behavior_tester->isActive()) {
+    drive_on_heading_behavior_tester->activate();
+  }
+
+  bool success = false;
+  success = drive_on_heading_behavior_tester->defaultDriveOnHeadingBehaviorTest(
+    goal,
+    tolerance);
+
+  float dist_to_obstacle = 2.0f;
+  if ( ((dist_to_obstacle - std::fabs(test_params.x)) < std::fabs(goal.speed)) ||
+    std::fabs(goal.target.y) > 0 ||
+    goal.time_allowance.sec < 2.0 ||
+    !((goal.target.x > 0.0) == (goal.speed > 0.0)))
+  {
+    EXPECT_FALSE(success);
+  } else {
+    EXPECT_TRUE(success);
+  }
+}
+
+std::vector<TestParameters> test_params = {TestParameters{-0.05, 0.0, -0.2, 10.0, 0.01},
+  TestParameters{-0.05, 0.1, -0.2, 10.0, 0.01},
+  TestParameters{-2.0, 0.0, -0.2, 10.0, 0.1},
+  TestParameters{-0.05, 0.0, -0.01, 1.0, 0.01},
+  TestParameters{0.05, 0.0, -0.2, 10.0, 0.01}};
+
+INSTANTIATE_TEST_SUITE_P(
+  DriveOnHeadingBehaviorTests,
+  DriveOnHeadingBehaviorTestFixture,
+  ::testing::Values(
+    test_params[0],
+    test_params[1],
+    test_params[2],
+    test_params[3],
+    test_params[4]),
+  testNameGenerator);
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_system_tests/src/behaviors/spin/CMakeLists.txt

@@ -0,0 +1,34 @@
+set(test_spin_behavior_exec test_spin_behavior_node)
+
+ament_add_gtest_executable(${test_spin_behavior_exec}
+  test_spin_behavior_node.cpp
+  spin_behavior_tester.cpp
+)
+
+ament_target_dependencies(${test_spin_behavior_exec}
+  ${dependencies}
+)
+
+ament_add_test(test_spin_behavior
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_behavior_exec}>
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+)
+
+ament_add_test(test_spin_behavior_fake
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_fake_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_behavior_exec}>
+    MAKE_FAKE_COSTMAP=true
+)

navigation2/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp

@@ -0,0 +1,351 @@
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#include <string>
+#include <random>
+#include <tuple>
+#include <memory>
+#include <iostream>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+
+#include "spin_behavior_tester.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_system_tests
+{
+
+SpinBehaviorTester::SpinBehaviorTester()
+: is_active_(false),
+  initial_pose_received_(false)
+{
+  node_ = rclcpp::Node::make_shared("spin_behavior_test");
+
+
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
+  if (std::getenv("MAKE_FAKE_COSTMAP") != NULL) {
+    // if this variable is set, make a fake costmap
+    make_fake_costmap_ = true;
+  } else {
+    make_fake_costmap_ = false;
+  }
+
+  client_ptr_ = rclcpp_action::create_client<Spin>(
+    node_->get_node_base_interface(),
+    node_->get_node_graph_interface(),
+    node_->get_node_logging_interface(),
+    node_->get_node_waitables_interface(),
+    "spin");
+
+  publisher_ =
+    node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
+  fake_costmap_publisher_ =
+    node_->create_publisher<nav2_msgs::msg::Costmap>(
+    "local_costmap/costmap_raw",
+    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+  fake_footprint_publisher_ = node_->create_publisher<geometry_msgs::msg::PolygonStamped>(
+    "local_costmap/published_footprint", rclcpp::SystemDefaultsQoS());
+
+  subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&SpinBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
+
+  stamp_ = node_->now();
+}
+
+SpinBehaviorTester::~SpinBehaviorTester()
+{
+  if (is_active_) {
+    deactivate();
+  }
+}
+
+void SpinBehaviorTester::activate()
+{
+  if (is_active_) {
+    throw std::runtime_error("Trying to activate while already active");
+    return;
+  }
+  if (!make_fake_costmap_) {
+    while (!initial_pose_received_) {
+      RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
+      sendInitialPose();
+      std::this_thread::sleep_for(100ms);
+      rclcpp::spin_some(node_);
+    }
+  } else {
+    sendFakeOdom(0.0);
+  }
+
+  // Wait for lifecycle_manager_navigation to activate behavior_server
+  std::this_thread::sleep_for(10s);
+
+  if (!client_ptr_) {
+    RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
+    is_active_ = false;
+    return;
+  }
+
+  if (!client_ptr_->wait_for_action_server(10s)) {
+    RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
+    is_active_ = false;
+    return;
+  }
+
+  RCLCPP_INFO(this->node_->get_logger(), "Spin action server is ready");
+  is_active_ = true;
+}
+
+void SpinBehaviorTester::deactivate()
+{
+  if (!is_active_) {
+    throw std::runtime_error("Trying to deactivate while already inactive");
+  }
+  is_active_ = false;
+}
+
+bool SpinBehaviorTester::defaultSpinBehaviorTest(
+  const float target_yaw,
+  const double tolerance)
+{
+  if (!is_active_) {
+    RCLCPP_ERROR(node_->get_logger(), "Not activated");
+    return false;
+  }
+
+  // Sleep to let behavior server be ready for serving in multiple runs
+  std::this_thread::sleep_for(5s);
+
+  if (make_fake_costmap_) {
+    sendFakeOdom(0.0);
+  }
+
+  auto goal_msg = Spin::Goal();
+  goal_msg.target_yaw = target_yaw;
+
+  // Initialize fake costmap
+  if (make_fake_costmap_) {
+    sendFakeCostmap(target_yaw);
+    sendFakeOdom(0.0);
+  }
+
+  geometry_msgs::msg::PoseStamped initial_pose;
+  if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) {
+    RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Found current robot pose");
+  RCLCPP_INFO(
+    node_->get_logger(),
+    "Init Yaw is %lf",
+    fabs(tf2::getYaw(initial_pose.pose.orientation)));
+  RCLCPP_INFO(node_->get_logger(), "Before sending goal");
+
+  // Initialize fake costmap
+  if (make_fake_costmap_) {
+    sendFakeCostmap(target_yaw);
+    sendFakeOdom(0.0);
+  }
+
+  rclcpp::sleep_for(std::chrono::milliseconds(100));
+
+  auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
+
+  if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<Spin>::SharedPtr goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
+    return false;
+  }
+
+  // Wait for the server to be done with the goal
+  auto result_future = client_ptr_->async_get_result(goal_handle);
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for result");
+  rclcpp::sleep_for(std::chrono::milliseconds(1000));
+
+  if (make_fake_costmap_) {  // if we are faking the costmap, we will fake success.
+    sendFakeOdom(0.0);
+    sendFakeCostmap(target_yaw);
+    RCLCPP_INFO(node_->get_logger(), "target_yaw %lf", target_yaw);
+    // Slowly increment command yaw by increment to simulate the robot slowly spinning into place
+    float step_size = tolerance / 4.0;
+    for (float command_yaw = 0.0;
+      abs(command_yaw) < abs(target_yaw);
+      command_yaw = command_yaw + step_size)
+    {
+      sendFakeOdom(command_yaw);
+      sendFakeCostmap(target_yaw);
+      rclcpp::sleep_for(std::chrono::milliseconds(1));
+    }
+    sendFakeOdom(target_yaw);
+    sendFakeCostmap(target_yaw);
+    RCLCPP_INFO(node_->get_logger(), "After sending goal");
+  }
+  if (rclcpp::spin_until_future_complete(node_, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<Spin>::WrappedResult wrapped_result = result_future.get();
+
+  switch (wrapped_result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED: break;
+    case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was aborted");
+      return false;
+    case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was canceled");
+      return false;
+    default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
+      return false;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "result received");
+
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) {
+    RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
+    return false;
+  }
+
+  double goal_yaw = angles::normalize_angle(
+    tf2::getYaw(initial_pose.pose.orientation) + target_yaw);
+  double dyaw = angles::shortest_angular_distance(
+    goal_yaw, tf2::getYaw(current_pose.pose.orientation));
+
+  if (fabs(dyaw) > tolerance) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "Init Yaw is %lf (tolerance %lf)",
+      fabs(tf2::getYaw(initial_pose.pose.orientation)), tolerance);
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "Current Yaw is %lf (tolerance %lf)",
+      fabs(tf2::getYaw(current_pose.pose.orientation)), tolerance);
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "Angular distance from goal is %lf (tolerance %lf)",
+      fabs(dyaw), tolerance);
+    return false;
+  }
+
+  return true;
+}
+
+void SpinBehaviorTester::sendFakeCostmap(float angle)
+{
+  nav2_msgs::msg::Costmap fake_costmap;
+
+  fake_costmap.header.frame_id = "odom";
+  fake_costmap.header.stamp = stamp_;
+  fake_costmap.metadata.layer = "master";
+  fake_costmap.metadata.resolution = .1;
+  fake_costmap.metadata.size_x = 100;
+  fake_costmap.metadata.size_y = 100;
+  fake_costmap.metadata.origin.position.x = 0;
+  fake_costmap.metadata.origin.position.y = 0;
+  fake_costmap.metadata.origin.orientation.w = 1.0;
+  float costmap_val = 0;
+  for (int ix = 0; ix < 100; ix++) {
+    for (int iy = 0; iy < 100; iy++) {
+      if (abs(angle) > M_PI_2f32) {
+        // fake obstacles in the way so we get failure due to potential collision
+        costmap_val = 100;
+      }
+      fake_costmap.data.push_back(costmap_val);
+    }
+  }
+  fake_costmap_publisher_->publish(fake_costmap);
+}
+
+void SpinBehaviorTester::sendInitialPose()
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped pose;
+  pose.header.frame_id = "map";
+  pose.header.stamp = stamp_;
+  pose.pose.pose.position.x = -2.0;
+  pose.pose.pose.position.y = -0.5;
+  pose.pose.pose.position.z = 0.0;
+  pose.pose.pose.orientation.x = 0.0;
+  pose.pose.pose.orientation.y = 0.0;
+  pose.pose.pose.orientation.z = 0.0;
+  pose.pose.pose.orientation.w = 1.0;
+  for (int i = 0; i < 35; i++) {
+    pose.pose.covariance[i] = 0.0;
+  }
+  pose.pose.covariance[0] = 0.08;
+  pose.pose.covariance[7] = 0.08;
+  pose.pose.covariance[35] = 0.05;
+
+  publisher_->publish(pose);
+  RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
+}
+
+void SpinBehaviorTester::sendFakeOdom(float angle)
+{
+  geometry_msgs::msg::TransformStamped transformStamped;
+
+  transformStamped.header.stamp = stamp_;
+  transformStamped.header.frame_id = "odom";
+  transformStamped.child_frame_id = "base_link";
+  transformStamped.transform.translation.x = 0.0;
+  transformStamped.transform.translation.y = 0.0;
+  transformStamped.transform.translation.z = 0.0;
+  tf2::Quaternion q;
+  q.setRPY(0, 0, angle);
+  transformStamped.transform.rotation.x = q.x();
+  transformStamped.transform.rotation.y = q.y();
+  transformStamped.transform.rotation.z = q.z();
+  transformStamped.transform.rotation.w = q.w();
+
+  tf_broadcaster_->sendTransform(transformStamped);
+
+  geometry_msgs::msg::PolygonStamped footprint;
+  footprint.header.frame_id = "odom";
+  footprint.header.stamp = stamp_;
+  footprint.polygon.points.resize(4);
+  footprint.polygon.points[0].x = 0.22;
+  footprint.polygon.points[0].y = 0.22;
+  footprint.polygon.points[1].x = 0.22;
+  footprint.polygon.points[1].y = -0.22;
+  footprint.polygon.points[2].x = -0.22;
+  footprint.polygon.points[2].y = -0.22;
+  footprint.polygon.points[3].x = -0.22;
+  footprint.polygon.points[3].y = 0.22;
+  fake_footprint_publisher_->publish(footprint);
+}
+void SpinBehaviorTester::amclPoseCallback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
+{
+  initial_pose_received_ = true;
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp

@@ -0,0 +1,108 @@
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#ifndef BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
+#define BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <thread>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "angles/angles.h"
+#include "nav2_msgs/action/spin.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "geometry_msgs/msg/point32.hpp"
+#include "geometry_msgs/msg/polygon_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+#include "tf2/utils.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+
+namespace nav2_system_tests
+{
+
+class SpinBehaviorTester
+{
+public:
+  using Spin = nav2_msgs::action::Spin;
+  using GoalHandleSpin = rclcpp_action::ClientGoalHandle<Spin>;
+
+  SpinBehaviorTester();
+  ~SpinBehaviorTester();
+
+  // Runs a single test with given target yaw
+  bool defaultSpinBehaviorTest(
+    float target_yaw,
+    double tolerance = 0.1);
+
+  void activate();
+
+  void deactivate();
+
+  bool isActive() const
+  {
+    return is_active_;
+  }
+
+private:
+  void sendInitialPose();
+
+  void sendFakeCostmap(float angle);
+  void sendFakeOdom(float angle);
+
+  void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
+
+  bool is_active_;
+  bool initial_pose_received_;
+  bool make_fake_costmap_;
+  rclcpp::Time stamp_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  // Publisher to publish initial pose
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
+
+  // Publisher to publish fake costmap raw
+  rclcpp::Publisher<nav2_msgs::msg::Costmap>::SharedPtr fake_costmap_publisher_;
+
+  // Publisher to publish fake costmap footprint
+  rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr fake_footprint_publisher_;
+
+  // Subscriber for amcl pose
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
+
+  // Action client to call spin action
+  rclcpp_action::Client<Spin>::SharedPtr client_ptr_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py

@@ -0,0 +1,161 @@
+#! /usr/bin/env python3
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+
+    namespace = LaunchConfiguration('namespace')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    autostart = LaunchConfiguration('autostart')
+    params_file = LaunchConfiguration('params_file')
+    default_nav_through_poses_bt_xml = LaunchConfiguration('default_nav_through_poses_bt_xml')
+    default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml')
+    map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {
+        'use_sim_time': use_sim_time,
+        'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml,
+        'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml,
+        'autostart': autostart,
+        'map_subscribe_transient_local': map_subscribe_transient_local}
+
+    configured_params = RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True)
+
+    lifecycle_nodes = ['behavior_server']
+
+    # Map fully qualified names to relative ones so the node's namespace can be prepended.
+    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+    # TODO(orduno) Substitute with `PushNodeRemapping`
+    #              https://github.com/ros2/launch_ros/issues/56
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='Top-level namespace'),
+
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+
+        DeclareLaunchArgument(
+            'autostart', default_value='true',
+            description='Automatically startup the nav2 stack'),
+
+        DeclareLaunchArgument(
+            'params_file',
+            default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+            description='Full path to the ROS2 parameters file to use'),
+
+        DeclareLaunchArgument(
+            'default_nav_through_poses_bt_xml',
+            default_value=os.path.join(
+                get_package_share_directory('nav2_bt_navigator'),
+                'behavior_trees', 'navigate_through_poses_w_replanning_and_recovery.xml'),
+            description='Full path to the behavior tree xml file to use'),
+
+        DeclareLaunchArgument(
+            'default_nav_to_pose_bt_xml',
+            default_value=os.path.join(
+                get_package_share_directory('nav2_bt_navigator'),
+                'behavior_trees', 'navigate_to_pose_w_replanning_and_recovery.xml'),
+            description='Full path to the behavior tree xml file to use'),
+
+        DeclareLaunchArgument(
+            'map_subscribe_transient_local', default_value='false',
+            description='Whether to set the map subscriber QoS to transient local'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        Node(
+            package='nav2_behaviors',
+            executable='behavior_server',
+            name='behavior_server',
+            output='screen',
+            parameters=[configured_params],
+            remappings=remappings),
+
+        Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager_navigation',
+            output='screen',
+            parameters=[{'use_sim_time': use_sim_time},
+                        {'autostart': autostart},
+                        {'node_names': lifecycle_nodes}]),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_spin_behavior_fake_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py

@@ -0,0 +1,102 @@
+#! /usr/bin/env python3
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites='',
+        convert_types=True)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': configured_params,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable], name='test_spin_behavior_node', output='screen',
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp

@@ -0,0 +1,109 @@
+// Copyright (c) 2020 Samsung Research
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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. Reserved.
+
+#include <cmath>
+#include <tuple>
+#include <string>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "spin_behavior_tester.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::SpinBehaviorTester;
+
+std::string testNameGenerator(const testing::TestParamInfo<std::tuple<float, float>> & param)
+{
+  std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string(
+    std::get<1>(param.param));
+  name.erase(std::remove(name.begin(), name.end(), '.'), name.end());
+  return name;
+}
+
+class SpinBehaviorTestFixture
+  : public ::testing::TestWithParam<std::tuple<float, float>>
+{
+public:
+  static void SetUpTestCase()
+  {
+    spin_recovery_tester = new SpinBehaviorTester();
+    if (!spin_recovery_tester->isActive()) {
+      spin_recovery_tester->activate();
+    }
+  }
+
+  static void TearDownTestCase()
+  {
+    delete spin_recovery_tester;
+    spin_recovery_tester = nullptr;
+  }
+
+protected:
+  static SpinBehaviorTester * spin_recovery_tester;
+};
+
+SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr;
+
+TEST_P(SpinBehaviorTestFixture, testSpinRecovery)
+{
+  float target_yaw = std::get<0>(GetParam());
+  float tolerance = std::get<1>(GetParam());
+
+  bool success = false;
+  int num_tries = 3;
+  for (int i = 0; i != num_tries; i++) {
+    success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance);
+    if (success) {
+      break;
+    }
+  }
+  if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) {
+    // if this variable is set, make a fake costmap
+    // in the fake spin test, we expect a collision for angles > M_PI_2
+    EXPECT_EQ(false, success);
+  } else {
+    EXPECT_EQ(true, success);
+  }
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  SpinRecoveryTests,
+  SpinBehaviorTestFixture,
+  ::testing::Values(
+    std::make_tuple(-M_PIf32 / 6.0, 0.1),
+    std::make_tuple(M_PI_4f32, 0.1),
+    std::make_tuple(-M_PI_2f32, 0.1),
+    std::make_tuple(M_PIf32, 0.1),
+    std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15),
+    std::make_tuple(-2.0 * M_PIf32, 0.1),
+    std::make_tuple(4.0 * M_PIf32, 0.15)),
+  testNameGenerator);
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_system_tests/src/behaviors/wait/CMakeLists.txt

@@ -0,0 +1,23 @@
+set(test_wait_behavior_exec test_wait_behavior_node)
+
+ament_add_gtest_executable(${test_wait_behavior_exec}
+  test_wait_behavior_node.cpp
+  wait_behavior_tester.cpp
+)
+
+ament_target_dependencies(${test_wait_behavior_exec}
+  ${dependencies}
+)
+
+ament_add_test(test_wait_behavior
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_behavior_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_wait_behavior_exec}>
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_behavior.xml
+)

navigation2/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py

@@ -0,0 +1,102 @@
+#! /usr/bin/env python3
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites='',
+        convert_types=True)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': configured_params,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable], name='test_wait_behavior_node', output='screen',
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/behaviors/wait/test_wait_behavior_node.cpp

@@ -0,0 +1,106 @@
+// Copyright (c) 2020 Samsung Research
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <tuple>
+#include <string>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "wait_behavior_tester.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::WaitBehaviorTester;
+
+std::string testNameGenerator(const testing::TestParamInfo<std::tuple<float, float>> & param)
+{
+  std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string(
+    std::get<1>(param.param));
+  name.erase(std::remove(name.begin(), name.end(), '.'), name.end());
+  return name;
+}
+
+class WaitBehaviorTestFixture
+  : public ::testing::TestWithParam<std::tuple<float, float>>
+{
+public:
+  static void SetUpTestCase()
+  {
+    wait_behavior_tester = new WaitBehaviorTester();
+    if (!wait_behavior_tester->isActive()) {
+      wait_behavior_tester->activate();
+    }
+  }
+
+  static void TearDownTestCase()
+  {
+    delete wait_behavior_tester;
+    wait_behavior_tester = nullptr;
+  }
+
+protected:
+  static WaitBehaviorTester * wait_behavior_tester;
+};
+
+WaitBehaviorTester * WaitBehaviorTestFixture::wait_behavior_tester = nullptr;
+
+TEST_P(WaitBehaviorTestFixture, testSWaitBehavior)
+{
+  float wait_time = std::get<0>(GetParam());
+  float cancel = std::get<1>(GetParam());
+
+  bool success = false;
+  int num_tries = 3;
+  for (int i = 0; i != num_tries; i++) {
+    if (cancel == 1.0) {
+      success = success || wait_behavior_tester->behaviorTestCancel(wait_time);
+    } else {
+      success = success || wait_behavior_tester->behaviorTest(wait_time);
+    }
+    if (success) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(true, success);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  WaitBehaviorTests,
+  WaitBehaviorTestFixture,
+  ::testing::Values(
+    std::make_tuple(1.0, 0.0),
+    std::make_tuple(2.0, 0.0),
+    std::make_tuple(5.0, 0.0),
+    std::make_tuple(10.0, 1.0)),
+  testNameGenerator);
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp

@@ -0,0 +1,280 @@
+// Copyright (c) 2020 Samsung Research
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#include <string>
+#include <random>
+#include <tuple>
+#include <memory>
+#include <iostream>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+
+#include "wait_behavior_tester.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_system_tests
+{
+
+WaitBehaviorTester::WaitBehaviorTester()
+: is_active_(false),
+  initial_pose_received_(false)
+{
+  node_ = rclcpp::Node::make_shared("wait_behavior_test");
+
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  client_ptr_ = rclcpp_action::create_client<Wait>(
+    node_->get_node_base_interface(),
+    node_->get_node_graph_interface(),
+    node_->get_node_logging_interface(),
+    node_->get_node_waitables_interface(),
+    "wait");
+
+  publisher_ =
+    node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
+
+  subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&WaitBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
+}
+
+WaitBehaviorTester::~WaitBehaviorTester()
+{
+  if (is_active_) {
+    deactivate();
+  }
+}
+
+void WaitBehaviorTester::activate()
+{
+  if (is_active_) {
+    throw std::runtime_error("Trying to activate while already active");
+    return;
+  }
+
+  while (!initial_pose_received_) {
+    RCLCPP_WARN(node_->get_logger(), "Initial pose not received");
+    sendInitialPose();
+    std::this_thread::sleep_for(100ms);
+    rclcpp::spin_some(node_);
+  }
+
+  // Wait for lifecycle_manager_navigation to activate behavior_server
+  std::this_thread::sleep_for(10s);
+
+  if (!client_ptr_) {
+    RCLCPP_ERROR(node_->get_logger(), "Action client not initialized");
+    is_active_ = false;
+    return;
+  }
+
+  if (!client_ptr_->wait_for_action_server(10s)) {
+    RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting");
+    is_active_ = false;
+    return;
+  }
+
+  RCLCPP_INFO(this->node_->get_logger(), "Wait action server is ready");
+  is_active_ = true;
+}
+
+void WaitBehaviorTester::deactivate()
+{
+  if (!is_active_) {
+    throw std::runtime_error("Trying to deactivate while already inactive");
+  }
+  is_active_ = false;
+}
+
+bool WaitBehaviorTester::behaviorTest(
+  const float wait_time)
+{
+  if (!is_active_) {
+    RCLCPP_ERROR(node_->get_logger(), "Not activated");
+    return false;
+  }
+
+  // Sleep to let behavior server be ready for serving in multiple runs
+  std::this_thread::sleep_for(5s);
+
+  auto start_time = node_->now();
+  auto goal_msg = Wait::Goal();
+  goal_msg.time = rclcpp::Duration(wait_time, 0.0);
+
+  RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
+
+  auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
+
+  if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
+    return false;
+  }
+
+  // Wait for the server to be done with the goal
+  auto result_future = client_ptr_->async_get_result(goal_handle);
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for result");
+  if (rclcpp::spin_until_future_complete(node_, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "get result call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<Wait>::WrappedResult wrapped_result = result_future.get();
+
+  switch (wrapped_result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED: break;
+    case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was aborted");
+      return false;
+    case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was canceled");
+      return false;
+    default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
+      return false;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "result received");
+
+  if ((node_->now() - start_time).seconds() < static_cast<double>(wait_time)) {
+    return false;
+  }
+
+  return true;
+}
+
+bool WaitBehaviorTester::behaviorTestCancel(
+  const float wait_time)
+{
+  if (!is_active_) {
+    RCLCPP_ERROR(node_->get_logger(), "Not activated");
+    return false;
+  }
+
+  // Sleep to let behavior server be ready for serving in multiple runs
+  std::this_thread::sleep_for(5s);
+
+  auto start_time = node_->now();
+  auto goal_msg = Wait::Goal();
+  goal_msg.time = rclcpp::Duration(wait_time, 0.0);
+
+  RCLCPP_INFO(this->node_->get_logger(), "Sending goal");
+
+  auto goal_handle_future = client_ptr_->async_send_goal(goal_msg);
+
+  if (rclcpp::spin_until_future_complete(node_, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "send goal call failed :(");
+    return false;
+  }
+
+  rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server");
+    return false;
+  }
+
+  // Wait for the server to be done with the goal
+  auto result_future = client_ptr_->async_cancel_all_goals();
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for cancellation");
+  if (rclcpp::spin_until_future_complete(node_, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "get cancel result call failed :(");
+    return false;
+  }
+
+  auto status = goal_handle_future.get()->get_status();
+
+  switch (status) {
+    case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal succeeded");
+      return false;
+    case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal was aborted");
+      return false;
+    case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO(
+        node_->get_logger(),
+        "Goal was canceled");
+      return true;
+    case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO(
+        node_->get_logger(),
+        "Goal is cancelling");
+      return true;
+    case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal is executing");
+      return false;
+    case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR(
+        node_->get_logger(),
+        "Goal is processing");
+      return false;
+    default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code");
+      return false;
+  }
+
+  return false;
+}
+
+void WaitBehaviorTester::sendInitialPose()
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped pose;
+  pose.header.frame_id = "map";
+  pose.header.stamp = rclcpp::Time();
+  pose.pose.pose.position.x = -2.0;
+  pose.pose.pose.position.y = -0.5;
+  pose.pose.pose.position.z = 0.0;
+  pose.pose.pose.orientation.x = 0.0;
+  pose.pose.pose.orientation.y = 0.0;
+  pose.pose.pose.orientation.z = 0.0;
+  pose.pose.pose.orientation.w = 1.0;
+  for (int i = 0; i < 35; i++) {
+    pose.pose.covariance[i] = 0.0;
+  }
+  pose.pose.covariance[0] = 0.08;
+  pose.pose.covariance[7] = 0.08;
+  pose.pose.covariance[35] = 0.05;
+
+  publisher_->publish(pose);
+  RCLCPP_INFO(node_->get_logger(), "Sent initial pose");
+}
+
+void WaitBehaviorTester::amclPoseCallback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr)
+{
+  initial_pose_received_ = true;
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp

@@ -0,0 +1,91 @@
+// Copyright (c) 2020 Samsung Research
+// Copyright (c) 2020 Sarthak Mittal
+// 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. Reserved.
+
+#ifndef BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
+#define BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <thread>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "angles/angles.h"
+#include "nav2_msgs/action/wait.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+
+#include "tf2/utils.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+namespace nav2_system_tests
+{
+
+class WaitBehaviorTester
+{
+public:
+  using Wait = nav2_msgs::action::Wait;
+  using GoalHandleWait = rclcpp_action::ClientGoalHandle<Wait>;
+
+  WaitBehaviorTester();
+  ~WaitBehaviorTester();
+
+  // Runs a single test with given target yaw
+  bool behaviorTest(
+    float time);
+
+  bool behaviorTestCancel(float time);
+
+  void activate();
+
+  void deactivate();
+
+  bool isActive() const
+  {
+    return is_active_;
+  }
+
+private:
+  void sendInitialPose();
+
+  void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
+
+  bool is_active_;
+  bool initial_pose_received_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  // Publisher to publish initial pose
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
+
+  // Subscriber for amcl pose
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
+
+  // Action client to call wait action
+  rclcpp_action::Client<Wait>::SharedPtr client_ptr_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_

navigation2/nav2_system_tests/src/costmap_filters/CMakeLists.txt

@@ -0,0 +1,47 @@
+ament_add_test(test_keepout_filter
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_keepout_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_MASK=${PROJECT_SOURCE_DIR}/maps/keepout_mask.yaml
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/keepout_params.yaml
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+    ASTAR=False
+)
+
+ament_add_test(test_speed_filter_global
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_speed_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_MASK=${PROJECT_SOURCE_DIR}/maps/speed_mask.yaml
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/speed_global_params.yaml
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+    ASTAR=False
+)
+
+ament_add_test(test_speed_filter_local
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_speed_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_MASK=${PROJECT_SOURCE_DIR}/maps/speed_mask.yaml
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/speed_local_params.yaml
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+    ASTAR=False
+)

navigation2/nav2_system_tests/src/costmap_filters/keepout_params.yaml

@@ -0,0 +1,333 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: true
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field_prob"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::OmniMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch
+    # files to allow for a commandline change default used is the
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml &
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_smooth_path_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_assisted_teleop_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_drive_on_heading_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_is_path_valid_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_truncate_path_local_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_goal_updated_controller_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_controller_cancel_bt_node
+    - nav2_path_longer_on_approach_bt_node
+    - nav2_wait_cancel_bt_node
+    - nav2_spin_cancel_bt_node
+    - nav2_back_up_cancel_bt_node
+    - nav2_assisted_teleop_cancel_bt_node
+    - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["goal_checker"]
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+      stateful: True
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      prune_distance: 1.0
+      forward_prune_distance: 1.0
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+      publish_cost_grid_pc: True
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel_layer", "inflation_layer"]
+      filters: ["keepout_filter"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      keepout_filter:
+        plugin: "nav2_costmap_2d::KeepoutFilter"
+        enabled: True
+        filter_info_topic: "/costmap_filter_info"
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      filters: ["keepout_filter"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      keepout_filter:
+        plugin: "nav2_costmap_2d::KeepoutFilter"
+        enabled: True
+        filter_info_topic: "/costmap_filter_info"
+      always_send_full_costmap: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    yaml_filename: "turtlebot3_world.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: False
+      allow_unknown: True
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "waypoint_task_executor"
+    waypoint_task_executor:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 0
+
+costmap_filter_info_server:
+  ros__parameters:
+    use_sim_time: true
+    type: 0
+    filter_info_topic: "/costmap_filter_info"
+    mask_topic: "/filter_mask"
+    base: 0.0
+    multiplier: 1.0
+
+filter_mask_server:
+  ros__parameters:
+    use_sim_time: true
+    frame_id: "map"
+    topic_name: "/filter_mask"
+    yaml_filename: "keepout_mask.yaml"

navigation2/nav2_system_tests/src/costmap_filters/speed_global_params.yaml

@@ -0,0 +1,324 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "beam"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch
+    # files to allow for a commandline change default used is the
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml &
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_smooth_path_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_assisted_teleop_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_drive_on_heading_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_globally_updated_goal_condition_bt_node
+    - nav2_is_path_valid_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_truncate_path_local_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_goal_updated_controller_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_controller_cancel_bt_node
+    - nav2_path_longer_on_approach_bt_node
+    - nav2_wait_cancel_bt_node
+    - nav2_spin_cancel_bt_node
+    - nav2_back_up_cancel_bt_node
+    - nav2_assisted_teleop_cancel_bt_node
+    - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    speed_limit_topic: "/speed_limit"
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["goal_checker"]
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+      stateful: True
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      filters: ["speed_filter"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      speed_filter:
+        plugin: "nav2_costmap_2d::SpeedFilter"
+        enabled: True
+        filter_info_topic: "/costmap_filter_info"
+        speed_limit_topic: "/speed_limit"
+      always_send_full_costmap: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    yaml_filename: "turtlebot3_world.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: False
+      allow_unknown: True
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "waypoint_task_executor"
+    waypoint_task_executor:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 0
+
+costmap_filter_info_server:
+  ros__parameters:
+    use_sim_time: true
+    type: 1
+    filter_info_topic: "/costmap_filter_info"
+    mask_topic: "/filter_mask"
+    base: 100.0
+    multiplier: -1.0
+
+filter_mask_server:
+  ros__parameters:
+    use_sim_time: true
+    frame_id: "map"
+    topic_name: "/filter_mask"
+    yaml_filename: "speed_mask.yaml"

navigation2/nav2_system_tests/src/costmap_filters/speed_local_params.yaml

@@ -0,0 +1,324 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "beam"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch
+    # files to allow for a commandline change default used is the
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml &
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_smooth_path_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_assisted_teleop_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_drive_on_heading_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_globally_updated_goal_condition_bt_node
+    - nav2_is_path_valid_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_truncate_path_local_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_goal_updated_controller_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_controller_cancel_bt_node
+    - nav2_path_longer_on_approach_bt_node
+    - nav2_wait_cancel_bt_node
+    - nav2_spin_cancel_bt_node
+    - nav2_back_up_cancel_bt_node
+    - nav2_assisted_teleop_cancel_bt_node
+    - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    speed_limit_topic: "/speed_limit"
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["goal_checker"]
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+      stateful: True
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel_layer", "inflation_layer"]
+      filters: ["speed_filter"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      static_layer:
+        map_subscribe_transient_local: True
+      speed_filter:
+        plugin: "nav2_costmap_2d::SpeedFilter"
+        enabled: True
+        filter_info_topic: "/costmap_filter_info"
+        speed_limit_topic: "/speed_limit"
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    yaml_filename: "turtlebot3_world.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: False
+      allow_unknown: True
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "waypoint_task_executor"
+    waypoint_task_executor:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 0
+
+costmap_filter_info_server:
+  ros__parameters:
+    use_sim_time: true
+    type: 1
+    filter_info_topic: "/costmap_filter_info"
+    mask_topic: "/filter_mask"
+    base: 100.0
+    multiplier: -1.0
+
+filter_mask_server:
+  ros__parameters:
+    use_sim_time: true
+    frame_id: "map"
+    topic_name: "/filter_mask"
+    yaml_filename: "speed_mask.yaml"

navigation2/nav2_system_tests/src/costmap_filters/test_keepout_launch.py

@@ -0,0 +1,149 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+# Copyright (c) 2020 Samsung Research Russia
+#
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_context import LaunchContext
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    filter_mask_file = os.getenv('TEST_MASK')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    script_dir = os.path.dirname(os.path.realpath(__file__))
+    params_file = os.path.join(script_dir, 'keepout_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    param_substitutions = {
+        'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'),
+        'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file,
+        'map_server.ros__parameters.yaml_filename': map_yaml_file}
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    context = LaunchContext()
+    new_yaml = configured_params.perform(context)
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager_filters',
+            output='screen',
+            parameters=[{
+                            'node_names':
+                            [
+                                'filter_mask_server', 'costmap_filter_info_server'
+                            ]
+                        },
+                        {'autostart': True}]),
+
+        # Nodes required for Costmap Filters configuration
+        Node(
+            package='nav2_map_server',
+            executable='map_server',
+            name='filter_mask_server',
+            output='screen',
+            parameters=[new_yaml]),
+
+        Node(
+            package='nav2_map_server',
+            executable='costmap_filter_info_server',
+            name='costmap_filter_info_server',
+            output='screen',
+            parameters=[new_yaml]),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={'namespace': '',
+                              'use_namespace': 'False',
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': new_yaml,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+
+        Node(
+            package='nav2_costmap_2d',
+            executable='nav2_costmap_2d_cloud',
+            name='costmap_2d_cloud',
+            output='screen',
+            remappings=[('voxel_grid', 'local_costmap/voxel_grid')]),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
+             '-t', 'keepout', '-r', '-2.0', '-0.5', '0.0', '-0.5'],
+        name='tester_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/costmap_filters/test_speed_launch.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+# Copyright (c) 2020 Samsung Research Russia
+#
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_context import LaunchContext
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    filter_mask_file = os.getenv('TEST_MASK')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.getenv('PARAMS_FILE')
+
+    # Replace the `use_astar` setting on the params file
+    param_substitutions = {
+        'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'),
+        'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file,
+        'map_server.ros__parameters.yaml_filename': map_yaml_file}
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True)
+    context = LaunchContext()
+    new_yaml = configured_params.perform(context)
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager_filters',
+            output='screen',
+            parameters=[{
+                            'node_names':
+                            [
+                                'filter_mask_server', 'costmap_filter_info_server'
+                            ]
+                        },
+                        {'autostart': True}]),
+
+        # Nodes required for Costmap Filters configuration
+        Node(
+            package='nav2_map_server',
+            executable='map_server',
+            name='filter_mask_server',
+            output='screen',
+            parameters=[new_yaml]),
+
+        Node(
+            package='nav2_map_server',
+            executable='costmap_filter_info_server',
+            name='costmap_filter_info_server',
+            output='screen',
+            parameters=[new_yaml]),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={'namespace': '',
+                              'use_namespace': 'False',
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': new_yaml,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
+             '-t', 'speed', '-r', '-2.0', '-0.5', '0.0', '-0.5'],
+        name='tester_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/costmap_filters/tester_node.py

@@ -0,0 +1,537 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation.
+# Copyright (c) 2020 Samsung Research Russia
+#
+# 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.
+
+import argparse
+from enum import Enum
+import math
+import sys
+import time
+from typing import Optional
+
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from lifecycle_msgs.srv import GetState
+from nav2_msgs.action import NavigateToPose
+from nav2_msgs.msg import SpeedLimit
+from nav2_msgs.srv import ManageLifecycleNodes
+from nav_msgs.msg import OccupancyGrid
+from nav_msgs.msg import Path
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+from sensor_msgs.msg import PointCloud2
+
+
+class TestType(Enum):
+    KEEPOUT = 0
+    SPEED = 1
+
+
+class FilterMask():
+
+    def __init__(
+        self,
+        filter_mask: OccupancyGrid
+    ):
+        self.filter_mask = filter_mask
+
+    # Converts world coordinates into filter mask map coordinate.
+    # Returns filter mask map coordinates or (-1, -1) in case
+    # if world coordinates are out of mask bounds.
+    def worldToMap(self, wx: float, wy: float):
+        origin_x = self.filter_mask.info.origin.position.x
+        origin_y = self.filter_mask.info.origin.position.y
+        size_x = self.filter_mask.info.width
+        size_y = self.filter_mask.info.height
+        resolution = self.filter_mask.info.resolution
+
+        if wx < origin_x or wy < origin_y:
+            return -1, -1
+
+        mx = int((wx - origin_x) / resolution)
+        my = int((wy - origin_y) / resolution)
+
+        if mx < size_x and my < size_y:
+            return mx, my
+
+        return -1, -1
+
+    # Gets filter_mask[mx, my] value
+    def getValue(self, mx, my):
+        size_x = self.filter_mask.info.width
+        return self.filter_mask.data[mx + my * size_x]
+
+
+class NavTester(Node):
+
+    def __init__(
+        self,
+        test_type: TestType,
+        initial_pose: Pose,
+        goal_pose: Pose,
+        namespace: str = ''
+    ):
+        super().__init__(node_name='nav2_tester', namespace=namespace)
+        self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
+                                                      'initialpose', 10)
+        self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
+
+        transient_local_qos = QoSProfile(
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=1)
+
+        volatile_qos = QoSProfile(
+            durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+            depth=1)
+
+        self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
+                                                       'amcl_pose', self.poseCallback,
+                                                       transient_local_qos)
+        self.clearing_ep_sub = self.create_subscription(PointCloud2,
+                                                        'local_costmap/clearing_endpoints',
+                                                        self.clearingEndpointsCallback,
+                                                        transient_local_qos)
+        self.test_type = test_type
+        self.filter_test_result = True
+        self.clearing_endpoints_received = False
+        self.voxel_marked_received = False
+        self.voxel_unknown_received = False
+        self.cost_cloud_received = False
+
+        if self.test_type == TestType.KEEPOUT:
+            self.plan_sub = self.create_subscription(Path, 'plan',
+                                                     self.planCallback, volatile_qos)
+            self.voxel_marked_sub = self.create_subscription(PointCloud2,
+                                                             'voxel_marked_cloud',
+                                                             self.voxelMarkedCallback,
+                                                             1)
+            self.voxel_unknown_sub = self.create_subscription(PointCloud2,
+                                                              'voxel_unknown_cloud',
+                                                              self.voxelUnknownCallback,
+                                                              1)
+            self.cost_cloud_sub = self.create_subscription(PointCloud2,
+                                                           'cost_cloud',
+                                                           self.dwbCostCloudCallback,
+                                                           1)
+        elif self.test_type == TestType.SPEED:
+            self.speed_it = 0
+            # Expected chain of speed limits
+            self.limits = [50.0, 0.0]
+            # Permissive array: all received speed limits must match to "limits" from above
+            self.limit_passed = [False, False]
+            self.plan_sub = self.create_subscription(SpeedLimit, 'speed_limit',
+                                                     self.speedLimitCallback, volatile_qos)
+
+        self.mask_received = False
+        self.mask_sub = self.create_subscription(OccupancyGrid, 'filter_mask',
+                                                 self.maskCallback, transient_local_qos)
+
+        self.initial_pose_received = False
+        self.initial_pose = initial_pose
+        self.goal_pose = goal_pose
+        self.action_client = ActionClient(
+            self, NavigateToPose, 'navigate_to_pose')
+
+    def info_msg(self, msg: str):
+        self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
+
+    def warn_msg(self, msg: str):
+        self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
+
+    def error_msg(self, msg: str):
+        self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
+
+    def setInitialPose(self):
+        msg = PoseWithCovarianceStamped()
+        msg.pose.pose = self.initial_pose
+        msg.header.frame_id = 'map'
+        self.info_msg('Publishing Initial Pose')
+        self.initial_pose_pub.publish(msg)
+        self.currentPose = self.initial_pose
+
+    def getStampedPoseMsg(self, pose: Pose):
+        msg = PoseStamped()
+        msg.header.frame_id = 'map'
+        msg.pose = pose
+        return msg
+
+    def publishGoalPose(self, goal_pose: Optional[Pose] = None):
+        self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
+        self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
+
+    def runNavigateAction(self, goal_pose: Optional[Pose] = None):
+        # Sends a `NavToPose` action request and waits for completion
+        self.info_msg("Waiting for 'NavigateToPose' action server")
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg(
+                "'NavigateToPose' action server not available, waiting...")
+
+        self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
+        goal_msg = NavigateToPose.Goal()
+        goal_msg.pose = self.getStampedPoseMsg(self.goal_pose)
+
+        self.info_msg('Sending goal request...')
+        send_goal_future = self.action_client.send_goal_async(goal_msg)
+
+        rclpy.spin_until_future_complete(self, send_goal_future)
+        goal_handle = send_goal_future.result()
+
+        if not goal_handle.accepted:
+            self.error_msg('Goal rejected')
+            return False
+
+        self.info_msg('Goal accepted')
+        get_result_future = goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'NavigateToPose' action to complete")
+        rclpy.spin_until_future_complete(self, get_result_future)
+        status = get_result_future.result().status
+        if status != GoalStatus.STATUS_SUCCEEDED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+
+        self.info_msg('Goal succeeded!')
+        return True
+
+    def isInKeepout(self, x, y):
+        mx, my = self.filter_mask.worldToMap(x, y)
+        if mx == -1 and my == -1:  # Out of mask's area
+            return False
+        if self.filter_mask.getValue(mx, my) == 100:  # Occupied
+            return True
+        return False
+
+    # Checks that (x, y) position does not belong to a keepout zone.
+    def checkKeepout(self, x, y):
+        if not self.mask_received:
+            self.warn_msg('Filter mask was not received')
+        elif self.isInKeepout(x, y):
+            self.filter_test_result = False
+            self.error_msg(f'Pose ({x}, {y}) belongs to keepout zone')
+            return False
+        return True
+
+    # Checks that currently received speed_limit is equal to the it-th item
+    # of expected speed "limits" array.
+    # If so, sets it-th item of permissive array "limit_passed" to be true.
+    # Otherwise it will be remained to be false.
+    # Also verifies that speed limit messages received no more than N-times
+    # (where N - is the length of "limits" array),
+    # otherwise sets overall "filter_test_result" to be false.
+    def checkSpeed(self, it, speed_limit):
+        if it >= len(self.limits):
+            self.error_msg('Got excess speed limit')
+            self.filter_test_result = False
+            return
+        if speed_limit == self.limits[it]:
+            self.limit_passed[it] = True
+        else:
+            self.error_msg('Incorrect speed limit received: ' + str(speed_limit) +
+                           ', but should be: ' + str(self.limits[it]))
+
+    def poseCallback(self, msg):
+        self.info_msg('Received amcl_pose')
+        self.current_pose = msg.pose.pose
+        self.initial_pose_received = True
+        if self.test_type == TestType.KEEPOUT:
+            if not self.checkKeepout(msg.pose.pose.position.x, msg.pose.pose.position.y):
+                self.error_msg('Robot goes into keepout zone')
+
+    def planCallback(self, msg):
+        self.info_msg('Received plan')
+        for pose in msg.poses:
+            if not self.checkKeepout(pose.pose.position.x, pose.pose.position.y):
+                self.error_msg('Path plan intersects with keepout zone')
+                return
+
+    def clearingEndpointsCallback(self, msg):
+        if len(msg.data) > 0:
+            self.clearing_endpoints_received = True
+
+    def voxelMarkedCallback(self, msg):
+        if len(msg.data) > 0:
+            self.voxel_marked_received = True
+
+    def voxelUnknownCallback(self, msg):
+        if len(msg.data) > 0:
+            self.voxel_unknown_received = True
+
+    def dwbCostCloudCallback(self, msg):
+        self.info_msg('Received cost_cloud points')
+        if len(msg.data) > 0:
+            self.cost_cloud_received = True
+
+    def speedLimitCallback(self, msg):
+        self.info_msg(f'Received speed limit: {msg.speed_limit}')
+        self.checkSpeed(self.speed_it, msg.speed_limit)
+        self.speed_it += 1
+
+    def maskCallback(self, msg):
+        self.info_msg('Received filter mask')
+        self.filter_mask = FilterMask(msg)
+        self.mask_received = True
+
+    def wait_for_filter_mask(self, timeout):
+        start_time = time.time()
+
+        while not self.mask_received:
+            self.info_msg('Waiting for filter mask to be received ...')
+            rclpy.spin_once(self, timeout_sec=1)
+            if (time.time() - start_time) > timeout:
+                self.error_msg('Time out to waiting filter mask')
+                return False
+        return True
+
+    def wait_for_pointcloud_subscribers(self, timeout):
+        start_time = time.time()
+        while not self.voxel_unknown_received or not self.voxel_marked_received \
+                or not self.clearing_endpoints_received:
+            self.info_msg(
+                'Waiting for voxel_marked_cloud/voxel_unknown_cloud/\
+                clearing_endpoints msg to be received ...')
+            rclpy.spin_once(self, timeout_sec=1)
+            if (time.time() - start_time) > timeout:
+                self.error_msg(
+                    'Time out to waiting for voxel_marked_cloud/voxel_unknown_cloud/\
+                    clearing_endpoints msgs')
+                return False
+        return True
+
+    def reachesGoal(self, timeout, distance):
+        goalReached = False
+        start_time = time.time()
+
+        while not goalReached:
+            rclpy.spin_once(self, timeout_sec=1)
+            if self.distanceFromGoal() < distance:
+                goalReached = True
+                self.info_msg('*** GOAL REACHED ***')
+                return True
+            elif timeout is not None:
+                if (time.time() - start_time) > timeout:
+                    self.error_msg('Robot timed out reaching its goal!')
+                    return False
+
+    def distanceFromGoal(self):
+        d_x = self.current_pose.position.x - self.goal_pose.position.x
+        d_y = self.current_pose.position.y - self.goal_pose.position.y
+        distance = math.sqrt(d_x * d_x + d_y * d_y)
+        self.info_msg(f'Distance from goal is: {distance}')
+        return distance
+
+    def wait_for_node_active(self, node_name: str):
+        # Waits for the node within the tester namespace to become active
+        self.info_msg(f'Waiting for {node_name} to become active')
+        node_service = f'{node_name}/get_state'
+        state_client = self.create_client(GetState, node_service)
+        while not state_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{node_service} service not available, waiting...')
+        req = GetState.Request()  # empty request
+        state = 'UNKNOWN'
+        while (state != 'active'):
+            self.info_msg(f'Getting {node_name} state...')
+            future = state_client.call_async(req)
+            rclpy.spin_until_future_complete(self, future)
+            if future.result() is not None:
+                state = future.result().current_state.label
+                self.info_msg(f'Result of get_state: {state}')
+            else:
+                self.error_msg('Exception while calling service: %r' %
+                               future.exception())
+            time.sleep(5)
+
+    def shutdown(self):
+        self.info_msg('Shutting down')
+        self.action_client.destroy()
+
+        transition_service = 'lifecycle_manager_navigation/manage_nodes'
+        mgr_client = self.create_client(
+            ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            self.info_msg('Shutting down navigation lifecycle manager...')
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+            self.info_msg(
+                'Shutting down navigation lifecycle manager complete.')
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+        transition_service = 'lifecycle_manager_localization/manage_nodes'
+        mgr_client = self.create_client(
+            ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            self.info_msg('Shutting down localization lifecycle manager...')
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+            self.info_msg(
+                'Shutting down localization lifecycle manager complete')
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+
+    def wait_for_initial_pose(self):
+        self.initial_pose_received = False
+        while not self.initial_pose_received:
+            self.info_msg('Setting initial pose')
+            self.setInitialPose()
+            self.info_msg('Waiting for amcl_pose to be received')
+            rclpy.spin_once(self, timeout_sec=1)
+
+
+def test_RobotMovesToGoal(robot_tester):
+    robot_tester.info_msg('Setting goal pose')
+    robot_tester.publishGoalPose()
+    robot_tester.info_msg('Waiting 60 seconds for robot to reach goal')
+    return robot_tester.reachesGoal(timeout=60, distance=0.5)
+
+
+# Tests that all received speed limits are correct:
+# If overall "filter_test_result" is true
+# checks that all items in "limit_passed" permissive array are also true.
+# In other words, it verifies that all speed limits are received
+# exactly (by count and values) as expected by "limits" array.
+def test_SpeedLimitsAllCorrect(robot_tester):
+    if not robot_tester.filter_test_result:
+        return False
+    for passed in robot_tester.limit_passed:
+        if not passed:
+            robot_tester.error_msg('Did not meet one of the speed limit')
+            return False
+    return True
+
+
+def run_all_tests(robot_tester):
+    # set transforms to use_sim_time
+    result = True
+    if (result):
+        robot_tester.wait_for_node_active('amcl')
+        robot_tester.wait_for_initial_pose()
+        robot_tester.wait_for_node_active('bt_navigator')
+        result = robot_tester.wait_for_filter_mask(10)
+    if (result):
+        result = robot_tester.runNavigateAction()
+
+    if robot_tester.test_type == TestType.KEEPOUT:
+        result = result and robot_tester.wait_for_pointcloud_subscribers(10)
+
+    if (result):
+        result = test_RobotMovesToGoal(robot_tester)
+
+    if (result):
+        if robot_tester.test_type == TestType.KEEPOUT:
+            result = robot_tester.filter_test_result
+            result = result and robot_tester.cost_cloud_received
+        elif robot_tester.test_type == TestType.SPEED:
+            result = test_SpeedLimitsAllCorrect(robot_tester)
+
+    # Add more tests here if desired
+
+    if (result):
+        robot_tester.info_msg('Test PASSED')
+    else:
+        robot_tester.error_msg('Test FAILED')
+
+    return result
+
+
+def fwd_pose(x=0.0, y=0.0, z=0.01):
+    initial_pose = Pose()
+    initial_pose.position.x = x
+    initial_pose.position.y = y
+    initial_pose.position.z = z
+    initial_pose.orientation.x = 0.0
+    initial_pose.orientation.y = 0.0
+    initial_pose.orientation.z = 0.0
+    initial_pose.orientation.w = 1.0
+    return initial_pose
+
+
+def get_tester(args):
+
+    # Requested tester for one robot
+    type_str = args.type
+    init_x, init_y, final_x, final_y = args.robot[0]
+    test_type = TestType.KEEPOUT  # Default value
+    if type_str == 'speed':
+        test_type = TestType.SPEED
+
+    tester = NavTester(
+        test_type,
+        initial_pose=fwd_pose(float(init_x), float(init_y)),
+        goal_pose=fwd_pose(float(final_x), float(final_y)))
+    tester.info_msg(
+        'Starting tester, robot going from ' + init_x + ', ' + init_y +
+        ' to ' + final_x + ', ' + final_y + '.')
+    return tester
+
+
+def main(argv=sys.argv[1:]):
+    # The robot(s) positions from the input arguments
+    parser = argparse.ArgumentParser(
+        description='System-level costmap filters tester node')
+    parser.add_argument('-t', '--type', type=str, action='store', dest='type',
+                        help='Type of costmap filter being tested.')
+    group = parser.add_mutually_exclusive_group(required=True)
+    group.add_argument('-r', '--robot', action='append', nargs=4,
+                       metavar=('init_x', 'init_y', 'final_x', 'final_y'),
+                       help='The robot starting and final positions.')
+
+    args, unknown = parser.parse_known_args()
+
+    rclpy.init()
+
+    # Create tester for the robot
+    tester = get_tester(args)
+
+    # wait a few seconds to make sure entire stacks are up
+    time.sleep(10)
+
+    passed = run_all_tests(tester)
+
+    # stop and shutdown the nav stack to exit cleanly
+    tester.shutdown()
+    tester.info_msg('Done Shutting Down.')
+
+    if not passed:
+        tester.info_msg('Exiting failed')
+        exit(1)
+    else:
+        tester.info_msg('Exiting passed')
+        exit(0)
+
+
+if __name__ == '__main__':
+    main()

navigation2/nav2_system_tests/src/dummy_controller/CMakeLists.txt

@@ -0,0 +1,13 @@
+add_executable(dummy_controller_node
+  src/dummy_controller/main.cpp
+  src/dummy_controller/dummy_controller.cpp
+)
+
+ament_target_dependencies(dummy_controller_node
+  rclcpp
+  std_msgs
+  nav2_util
+  nav2_behavior_tree
+  nav2_msgs
+  nav_msgs
+)

navigation2/nav2_system_tests/src/dummy_controller/dummy_controller.cpp

@@ -0,0 +1,104 @@
+// 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 <chrono>
+#include <ctime>
+#include <thread>
+#include <memory>
+#include <utility>
+
+#include "dummy_controller.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_system_tests
+{
+
+DummyController::DummyController()
+: Node("DummyController")
+{
+  RCLCPP_INFO(get_logger(), "Initializing DummyController...");
+
+  auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
+
+  vel_pub_ =
+    this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+
+  task_server_ = std::make_unique<nav2_behavior_tree::FollowPathTaskServer>(temp_node, false),
+  task_server_->setExecuteCallback(
+    std::bind(&DummyController::followPath, this, std::placeholders::_1));
+
+  // Start listening for incoming ComputePathToPose action server requests
+  task_server_->start();
+
+  RCLCPP_INFO(get_logger(), "Initialized DummyController");
+}
+
+DummyController::~DummyController()
+{
+  RCLCPP_INFO(get_logger(), "Shutting down DummyController");
+}
+
+void
+DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedPtr /*command*/)
+{
+  RCLCPP_INFO(get_logger(), "Starting controller ");
+
+  auto start_time = std::chrono::system_clock::now();
+  auto time_since_msg = std::chrono::system_clock::now();
+
+  while (true) {
+    // Dummy controller computation time
+    std::this_thread::sleep_for(50ms);
+
+    if (task_server_->cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "Task cancelled");
+      setZeroVelocity();
+      task_server_->setCanceled();
+      return;
+    }
+
+    // Log a message every second
+    auto current_time = std::chrono::system_clock::now();
+    if (current_time - time_since_msg >= 1s) {
+      RCLCPP_INFO(get_logger(), "Following path");
+      time_since_msg = std::chrono::system_clock::now();
+    }
+
+    // Output control command
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
+    cmd_vel->linear.x = 0.1;
+    vel_pub_->publish(std::move(cmd_vel));
+
+    if (current_time - start_time >= 30s) {
+      RCLCPP_INFO(get_logger(), "Reached end point");
+      setZeroVelocity();
+      break;
+    }
+  }
+
+  nav2_behavior_tree::FollowPathResult result;
+  task_server_->setResult(result);
+}
+
+void DummyController::setZeroVelocity()
+{
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
+  cmd_vel->linear.x = 0.0;
+  cmd_vel->linear.y = 0.0;
+  cmd_vel->angular.z = 0.0;
+  vel_pub_->publish(std::move(cmd_vel));
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/dummy_controller/dummy_controller.hpp

@@ -0,0 +1,45 @@
+// 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 DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
+#define DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
+
+#include <memory>
+
+#include "nav2_behavior_tree/follow_path_task.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace nav2_system_tests
+{
+
+class DummyController : public rclcpp::Node
+{
+public:
+  DummyController();
+  ~DummyController();
+
+  nav2_behavior_tree::TaskStatus followPath(
+    const nav2_behavior_tree::FollowPathCommand::SharedPtr command);
+
+private:
+  void setZeroVelocity();
+
+  std::unique_ptr<nav2_behavior_tree::FollowPathTaskServer> task_server_;
+
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> vel_pub_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_

navigation2/nav2_system_tests/src/dummy_controller/main.cpp

@@ -0,0 +1,26 @@
+// 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. Reserved.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "dummy_controller.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_system_tests::DummyController>());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_system_tests/src/dummy_planner/CMakeLists.txt

@@ -0,0 +1,13 @@
+add_executable(dummy_planner_node
+  src/dummy_planner/main.cpp
+  src/dummy_planner/dummy_planner.cpp
+)
+
+ament_target_dependencies(dummy_planner_node
+  rclcpp
+  std_msgs
+  nav2_util
+  nav2_behavior_tree
+  nav2_msgs
+  nav_msgs
+)

navigation2/nav2_system_tests/src/dummy_planner/dummy_planner.cpp

@@ -0,0 +1,73 @@
+// 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 <chrono>
+#include <thread>
+#include <memory>
+
+#include "dummy_planner.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_system_tests
+{
+
+DummyPlanner::DummyPlanner()
+: Node("DummyPlanner")
+{
+  RCLCPP_INFO(get_logger(), "Initializing DummyPlanner...");
+
+  auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
+
+  task_server_ =
+    std::make_unique<nav2_behavior_tree::ComputePathToPoseTaskServer>(temp_node, false),
+  task_server_->setExecuteCallback(
+    std::bind(&DummyPlanner::computePlan, this, std::placeholders::_1));
+
+  // Start listening for incoming ComputePathToPose task requests
+  task_server_->start();
+
+  RCLCPP_INFO(get_logger(), "Initialized DummyPlanner");
+}
+
+DummyPlanner::~DummyPlanner()
+{
+  RCLCPP_INFO(get_logger(), "Shutting down DummyPlanner");
+}
+
+void
+DummyPlanner::computePlan(const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr cmd)
+{
+  RCLCPP_INFO(
+    get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
+    "(%.2f, %.2f).", cmd->start.position.x, cmd->start.position.y,
+    cmd->goal.position.x, cmd->goal.position.y);
+
+  // Dummy path computation time
+  std::this_thread::sleep_for(500ms);
+
+  if (task_server_->cancelRequested()) {
+    RCLCPP_INFO(get_logger(), "Cancelled planning task.");
+    task_server_->setCanceled();
+    return;
+  }
+
+  RCLCPP_INFO(get_logger(), "Found a dummy path");
+
+  nav2_behavior_tree::ComputePathToPoseResult result;
+  // set succeeded
+  task_server_->setResult(result);
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/dummy_planner/dummy_planner.hpp

@@ -0,0 +1,40 @@
+// 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 DUMMY_PLANNER__DUMMY_PLANNER_HPP_
+#define DUMMY_PLANNER__DUMMY_PLANNER_HPP_
+
+#include <memory>
+
+#include "nav2_behavior_tree/compute_path_to_pose_task.hpp"
+
+namespace nav2_system_tests
+{
+
+class DummyPlanner : public rclcpp::Node
+{
+public:
+  DummyPlanner();
+  ~DummyPlanner();
+
+  nav2_behavior_tree::TaskStatus computePathToPose(
+    const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr command);
+
+private:
+  std::unique_ptr<nav2_behavior_tree::ComputePathToPoseTaskServer> task_server_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // DUMMY_PLANNER__DUMMY_PLANNER_HPP_

navigation2/nav2_system_tests/src/dummy_planner/main.cpp

@@ -0,0 +1,26 @@
+// 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. Reserved.
+
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "dummy_planner.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_system_tests::DummyPlanner>());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_system_tests/src/localization/CMakeLists.txt

@@ -0,0 +1,18 @@
+ament_add_gtest_executable(test_localization_node
+  test_localization_node.cpp
+)
+ament_target_dependencies(test_localization_node
+  ${dependencies}
+)
+
+
+ament_add_test(test_localization
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_localization_launch.py"
+  TIMEOUT 180
+  ENV
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_EXECUTABLE=$<TARGET_FILE:test_localization_node>
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+)

navigation2/nav2_system_tests/src/localization/README.md

@@ -0,0 +1,22 @@
+# Localization Testing
+
+The intention of the localization test is to ensure robot’s pose and transforms are available.
+
+Currently, only a simple test that checks the `initialpose` has been implemented.  The `test_localization` module publishes an initial pose on `initialpose` topic and then it listens to `amcl_pose` topic. If the `amcl_pose` is similar to `initial pose` within a predefined tolerance the test passes.
+
+## To run the test
+First, build the package
+```
+colcon build --symlink-install
+```
+After building, from /build/nav2_system_tests directory run: 
+```
+ctest -V -R test_localization
+```
+Alternately you can run all the tests in the package using colcon:
+```
+colcon test --packages-select nav2_system_tests
+```
+
+## Future Plan
+Once rosbag functionality becomes available, this test can be extended to utilize a recorded trajectory with map and scan data to monitor the `amcl_pose` and `transforms` without a need to run Gazebo and map_server.

navigation2/nav2_system_tests/src/localization/test_localization_launch.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+
+# 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.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+import launch.actions
+from launch.actions import ExecuteProcess
+import launch_ros.actions
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    mapFile = os.getenv('TEST_MAP')
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+    world = os.getenv('TEST_WORLD')
+
+    launch_gazebo = launch.actions.ExecuteProcess(
+        cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world],
+        output='screen')
+    link_footprint = launch_ros.actions.Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        output='screen',
+        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'])
+    footprint_scan = launch_ros.actions.Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        output='screen',
+        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'])
+    run_map_server = launch_ros.actions.Node(
+        package='nav2_map_server',
+        executable='map_server',
+        name='map_server',
+        output='screen',
+        parameters=[{'yaml_filename': mapFile}])
+    run_amcl = launch_ros.actions.Node(
+        package='nav2_amcl',
+        executable='amcl',
+        output='screen')
+    run_lifecycle_manager = launch_ros.actions.Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager',
+        output='screen',
+        parameters=[{'node_names': ['map_server', 'amcl']}, {'autostart': True}])
+    ld = LaunchDescription([launch_gazebo, link_footprint, footprint_scan,
+                            run_map_server, run_amcl, run_lifecycle_manager])
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_localization_node',
+        output='screen'
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/localization/test_localization_node.cpp

@@ -0,0 +1,122 @@
+// 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 "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_amcl/amcl_node.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "geometry_msgs/msg/pose_array.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+// rclcpp::init can only be called once per process, so this needs to be a global variable
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class TestAmclPose : public ::testing::Test
+{
+public:
+  TestAmclPose()
+  {
+    pose_callback_ = false;
+    initTestPose();
+    tol_ = 0.25;
+
+    node = rclcpp::Node::make_shared("localization_test");
+
+    while (node->count_subscribers("scan") < 1) {
+      std::this_thread::sleep_for(100ms);
+      rclcpp::spin_some(node);
+    }
+
+    initial_pose_pub_ = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "initialpose", rclcpp::SystemDefaultsQoS());
+    subscription_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+      std::bind(&TestAmclPose::amcl_pose_callback, this, _1));
+    initial_pose_pub_->publish(testPose_);
+  }
+
+  bool defaultAmclTest();
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  void initTestPose();
+
+private:
+  void amcl_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+  {
+    auto amcl_pose = msg->pose;
+    amcl_pose_x = amcl_pose.pose.position.x;
+    amcl_pose_y = amcl_pose.pose.position.y;
+    pose_callback_ = true;
+  }
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
+  geometry_msgs::msg::PoseWithCovarianceStamped testPose_;
+  double amcl_pose_x;
+  double amcl_pose_y;
+  bool pose_callback_;
+  float tol_;
+};
+
+bool TestAmclPose::defaultAmclTest()
+{
+  initial_pose_pub_->publish(testPose_);
+  while (!pose_callback_) {
+    // TODO(mhpanah): Initial pose should only be published once.
+    initial_pose_pub_->publish(testPose_);
+    std::this_thread::sleep_for(1s);
+    rclcpp::spin_some(node);
+  }
+  if (std::abs(amcl_pose_x - testPose_.pose.pose.position.x) < tol_ &&
+    std::abs(amcl_pose_y - testPose_.pose.pose.position.y) < tol_)
+  {
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void TestAmclPose::initTestPose()
+{
+  testPose_.header.frame_id = "map";
+  testPose_.header.stamp = rclcpp::Time();
+  testPose_.pose.pose.position.x = -2.0;
+  testPose_.pose.pose.position.y = -0.5;
+  testPose_.pose.pose.position.z = 0.0;
+  testPose_.pose.pose.orientation.x = 0.0;
+  testPose_.pose.pose.orientation.y = 0.0;
+  testPose_.pose.pose.orientation.z = 0.0;
+  testPose_.pose.pose.orientation.w = 1.0;
+  for (int i = 0; i < 35; i++) {
+    testPose_.pose.covariance[i] = 0.0;
+  }
+  testPose_.pose.covariance[0] = 0.08;
+  testPose_.pose.covariance[7] = 0.08;
+  testPose_.pose.covariance[35] = 0.05;
+}
+
+TEST_F(TestAmclPose, SimpleAmclTest)
+{
+  EXPECT_EQ(true, defaultAmclTest());
+}

navigation2/nav2_system_tests/src/planning/CMakeLists.txt

@@ -0,0 +1,63 @@
+set(test_planner_costmaps_exec test_planner_costmaps_node)
+
+ament_add_gtest_executable(${test_planner_costmaps_exec}
+  test_planner_costmaps_node.cpp
+  planner_tester.cpp
+)
+
+target_link_libraries(${test_planner_costmaps_exec}
+  ${nav2_map_server_LIBRARIES})
+
+ament_target_dependencies(${test_planner_costmaps_exec}
+  ${dependencies}
+)
+
+set(test_planner_random_exec test_planner_random_node)
+
+ament_add_gtest_executable(${test_planner_random_exec}
+  test_planner_random_node.cpp
+  planner_tester.cpp
+)
+
+ament_target_dependencies(${test_planner_random_exec}
+  ${dependencies}
+)
+
+target_link_libraries(${test_planner_random_exec}
+  ${nav2_map_server_LIBRARIES})
+
+ament_add_test(test_planner_costmaps
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_costmaps_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_planner_costmaps_exec}>
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm
+)
+
+ament_add_test(test_planner_random
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_random_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:${test_planner_random_exec}>
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm
+)
+
+ament_add_gtest(test_planner_plugins
+  test_planner_plugins.cpp
+  TIMEOUT 10
+)
+
+ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies})
+target_link_libraries(test_planner_plugins
+  # stdc++fs
+)
+
+ament_add_gtest(test_planner_is_path_valid
+              test_planner_is_path_valid.cpp
+              planner_tester.cpp)
+
+ament_target_dependencies(test_planner_is_path_valid rclcpp geometry_msgs nav2_msgs ${dependencies})

navigation2/nav2_system_tests/src/planning/README.md

@@ -0,0 +1,15 @@
+# Global Planner Component Testing
+
+A PlannerTester node provides the world representation in the form of a costmap, sends a request to generate a path, and receives and checks the quality of the generated path.
+
+As mentioned above, currently the world is represented as a costmap. Simplified versions of the world model and costmap are used for testing.
+
+PlannerTester can sequentially pass random starting and goal poses and check the returned path for possible collision along the path.
+
+Below is an example of the output from randomized testing. Blue spheres represent the starting locations, green, the goals. Red lines are the computed paths. Grey cells represent obstacles.
+
+![alt text](example_result.png "Output Example")
+
+*Note: Currently robot size is 1x1 cells, no obstacle inflation is done on the costmap*
+
+*Note: The Navfn algorithm sometimes fails to generate a path as you can see from the 'orphan' spheres.*

navigation2/nav2_system_tests/src/planning/planner_tester.cpp

@@ -0,0 +1,513 @@
+// 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. Reserved.
+
+#include <string>
+#include <random>
+#include <tuple>
+#include <utility>
+#include <vector>
+#include <memory>
+#include <iostream>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+
+#include "planner_tester.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "nav2_map_server/map_mode.hpp"
+#include "nav2_map_server/map_io.hpp"
+#include "nav2_msgs/msg/costmap_meta_data.hpp"
+
+using namespace std::chrono_literals;
+using namespace std::chrono;  // NOLINT
+using nav2_util::Costmap;
+using nav2_util::TestCostmap;
+
+namespace nav2_system_tests
+{
+
+PlannerTester::PlannerTester()
+: Node("PlannerTester"), is_active_(false),
+  map_set_(false), costmap_set_(false),
+  using_fake_costmap_(true), trinary_costmap_(true),
+  track_unknown_space_(false), lethal_threshold_(100), unknown_cost_value_(-1),
+  testCostmapType_(TestCostmap::open_space), base_transform_(nullptr),
+  map_publish_rate_(100s)
+{
+}
+
+void PlannerTester::activate()
+{
+  if (is_active_) {
+    throw std::runtime_error("Trying to activate while already active");
+    return;
+  }
+  is_active_ = true;
+
+  // Launch a thread to process the messages for this node
+  spin_thread_ = std::make_unique<nav2_util::NodeThread>(this);
+
+  // We start with a 10x10 grid with no obstacles
+  costmap_ = std::make_unique<Costmap>(this);
+  loadSimpleCostmap(TestCostmap::open_space);
+
+  startRobotTransform();
+
+  // The navfn wrapper
+  auto state = rclcpp_lifecycle::State();
+  planner_tester_ = std::make_shared<NavFnPlannerTester>();
+  planner_tester_->declare_parameter(
+    "GridBased.use_astar", rclcpp::ParameterValue(true));
+  planner_tester_->set_parameter(
+    rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true)));
+  planner_tester_->set_parameter(
+    rclcpp::Parameter(std::string("expected_planner_frequency"), rclcpp::ParameterValue(-1.0)));
+  planner_tester_->onConfigure(state);
+  publishRobotTransform();
+  map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("map", 1);
+  path_valid_client_ = this->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
+  rclcpp::Rate r(1);
+  r.sleep();
+  planner_tester_->onActivate(state);
+}
+
+void PlannerTester::deactivate()
+{
+  if (!is_active_) {
+    throw std::runtime_error("Trying to deactivate while already inactive");
+    return;
+  }
+  is_active_ = false;
+
+  spin_thread_.reset();
+
+  auto state = rclcpp_lifecycle::State();
+  planner_tester_->onDeactivate(state);
+  planner_tester_->onCleanup(state);
+
+  map_timer_.reset();
+  map_pub_.reset();
+  map_.reset();
+  tf_broadcaster_.reset();
+}
+
+PlannerTester::~PlannerTester()
+{
+  if (is_active_) {
+    deactivate();
+  }
+}
+
+void PlannerTester::startRobotTransform()
+{
+  // Provide the robot pose transform
+  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+
+  // Set an initial pose
+  geometry_msgs::msg::Point robot_position;
+  robot_position.x = 1.0;
+  robot_position.y = 1.0;
+  updateRobotPosition(robot_position);
+
+  // Publish the transform periodically
+  transform_timer_ = create_wall_timer(
+    100ms, std::bind(&PlannerTester::publishRobotTransform, this));
+}
+
+void PlannerTester::updateRobotPosition(const geometry_msgs::msg::Point & position)
+{
+  if (!base_transform_) {
+    base_transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
+    base_transform_->header.frame_id = "map";
+    base_transform_->child_frame_id = "base_link";
+  }
+  std::cout << now().nanoseconds() << std::endl;
+
+  base_transform_->header.stamp = now() + rclcpp::Duration(0.25s);
+  base_transform_->transform.translation.x = position.x;
+  base_transform_->transform.translation.y = position.y;
+  base_transform_->transform.rotation.w = 1.0;
+
+  publishRobotTransform();
+}
+
+void PlannerTester::publishRobotTransform()
+{
+  if (base_transform_) {
+    tf_broadcaster_->sendTransform(*base_transform_);
+  }
+}
+
+void PlannerTester::loadDefaultMap()
+{
+  // Specs for the default map
+  double resolution = 1.0;
+  bool negate = false;
+  double occupancy_threshold = 0.65;
+  double free_threshold = 0.196;
+
+  // Define origin offset
+  std::vector<double> origin = {0.0, 0.0, 0.0};
+
+  nav2_map_server::MapMode mode = nav2_map_server::MapMode::Trinary;
+
+  std::string file_path = "";
+  char const * path = getenv("TEST_MAP");
+  if (path == NULL) {
+    throw std::runtime_error(
+            "Path to map image file"
+            " has not been specified in environment variable `TEST_MAP`.");
+  } else {
+    file_path = std::string(path);
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Loading map with file_path: %s", file_path.c_str());
+
+  try {
+    map_ = std::make_shared<nav_msgs::msg::OccupancyGrid>();
+
+    nav2_map_server::LoadParameters load_parameters;
+    load_parameters.image_file_name = file_path;
+    load_parameters.resolution = resolution;
+    load_parameters.origin = origin;
+    load_parameters.free_thresh = free_threshold;
+    load_parameters.occupied_thresh = occupancy_threshold;
+    load_parameters.mode = mode;
+    load_parameters.negate = negate;
+    loadMapFromFile(load_parameters, *map_);
+  } catch (...) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Failed to load image from file: %s", file_path.c_str());
+    throw;
+  }
+
+  map_->header.stamp = this->now();
+  map_->header.frame_id = "map";
+  map_->info.map_load_time = this->now();
+
+  // TODO(orduno): #443 replace with a latched topic
+  map_timer_ = create_wall_timer(1s, [this]() -> void {map_pub_->publish(*map_);});
+
+  map_set_ = true;
+  costmap_set_ = false;
+  using_fake_costmap_ = false;
+
+  setCostmap();
+}
+
+void PlannerTester::loadSimpleCostmap(const TestCostmap & testCostmapType)
+{
+  RCLCPP_INFO(get_logger(), "loadSimpleCostmap called.");
+  if (costmap_set_) {
+    RCLCPP_DEBUG(this->get_logger(), "Setting a new costmap with fake values");
+  }
+
+  costmap_->set_test_costmap(testCostmapType);
+
+  costmap_set_ = true;
+  using_fake_costmap_ = true;
+}
+
+void PlannerTester::setCostmap()
+{
+  if (!map_set_) {
+    RCLCPP_ERROR(this->get_logger(), "Map has not been provided");
+    return;
+  }
+
+  costmap_ = std::make_unique<Costmap>(
+    this, trinary_costmap_, track_unknown_space_, lethal_threshold_, unknown_cost_value_);
+
+  costmap_->set_static_map(*map_);
+
+  costmap_set_ = true;
+  using_fake_costmap_ = false;
+}
+
+bool PlannerTester::defaultPlannerTest(
+  ComputePathToPoseResult & path,
+  const double /*deviation_tolerance*/)
+{
+  if (!costmap_set_) {
+    RCLCPP_ERROR(this->get_logger(), "Costmap must be set before requesting a plan");
+    return false;
+  }
+
+  // TODO(orduno) #443 Add support for planners that take into account robot orientation
+  geometry_msgs::msg::Point robot_position;
+  ComputePathToPoseCommand goal;
+  auto costmap_properties = costmap_->get_properties();
+
+  if (using_fake_costmap_) {
+    RCLCPP_DEBUG(this->get_logger(), "Planning using a fake costmap");
+
+    robot_position.x = 1.0;
+    robot_position.y = 1.0;
+
+    goal.pose.position.x = 8.0;
+    goal.pose.position.y = 8.0;
+
+  } else {
+    RCLCPP_DEBUG(this->get_logger(), "Planning using the provided map");
+
+    // Defined with respect to world coordinate system
+    //  Planner will do coordinate transformation to map internally
+    robot_position.x = 390.0;
+    robot_position.y = 10.0;
+
+    goal.pose.position.x = 10.0;
+    goal.pose.position.y = 390.0;
+  }
+
+  // TODO(orduno): #443 On a default test, provide the reference path to compare with the planner
+  //               result.
+  return plannerTest(robot_position, goal, path);
+}
+
+bool PlannerTester::defaultPlannerRandomTests(
+  const unsigned int number_tests,
+  const float acceptable_fail_ratio = 0.1)
+{
+  if (!costmap_set_) {
+    RCLCPP_ERROR(this->get_logger(), "Costmap must be set before requesting a plan");
+    return false;
+  }
+
+  if (using_fake_costmap_) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Randomized testing with hardcoded costmaps not implemented yet");
+    return false;
+  }
+
+  // Initialize random number generator
+  std::random_device random_device;
+  std::mt19937 generator(random_device());
+
+  // Obtain random positions within map
+  std::uniform_int_distribution<> distribution_x(1, costmap_->get_properties().size_x - 1);
+  std::uniform_int_distribution<> distribution_y(1, costmap_->get_properties().size_y - 1);
+
+  auto generate_random = [&]() mutable -> std::pair<int, int> {
+      bool point_is_free = false;
+      int x, y;
+      while (!point_is_free) {
+        x = distribution_x(generator);
+        y = distribution_y(generator);
+        point_is_free = costmap_->is_free(x, y);
+      }
+      return std::make_pair(x, y);
+    };
+
+  // TODO(orduno) #443 Add support for planners that take into account robot orientation
+  geometry_msgs::msg::Point robot_position;
+  ComputePathToPoseCommand goal;
+  ComputePathToPoseResult path;
+
+  unsigned int num_fail = 0;
+  auto start = high_resolution_clock::now();
+  for (unsigned int test_num = 0; test_num < number_tests; ++test_num) {
+    RCLCPP_DEBUG(this->get_logger(), "Running test #%u", test_num + 1);
+
+    // Compose the robot start position and goal using random numbers
+    // Defined with respect to world coordinate system
+    // Planner will do coordinate transformation to map internally
+
+    auto vals = generate_random();
+    robot_position.x = vals.first;
+    robot_position.y = vals.second;
+
+    vals = generate_random();
+    goal.pose.position.x = vals.first;
+    goal.pose.position.y = vals.second;
+
+    if (!plannerTest(robot_position, goal, path)) {
+      RCLCPP_WARN(
+        this->get_logger(), "Failed with start at %0.2f, %0.2f and goal at %0.2f, %0.2f",
+        robot_position.x, robot_position.y, goal.pose.position.x, goal.pose.position.y);
+      ++num_fail;
+    }
+  }
+  auto end = high_resolution_clock::now();
+  auto elapsed = duration_cast<milliseconds>(end - start);
+
+  RCLCPP_INFO(
+    this->get_logger(),
+    "Tested with %u tests. Planner failed on %u. Test time %ld ms",
+    number_tests, num_fail, elapsed.count());
+
+  if ((num_fail / number_tests) > acceptable_fail_ratio) {
+    return false;
+  }
+
+  return true;
+}
+
+bool PlannerTester::plannerTest(
+  const geometry_msgs::msg::Point & robot_position,
+  const ComputePathToPoseCommand & goal,
+  ComputePathToPoseResult & path)
+{
+  RCLCPP_DEBUG(this->get_logger(), "Getting the path from the planner");
+
+  // First make available the current robot position for the planner to take as starting point
+  updateRobotPosition(robot_position);
+  sleep(0.05);
+
+  // Then request to compute a path
+  TaskStatus status = createPlan(goal, path);
+
+  RCLCPP_DEBUG(this->get_logger(), "Path request status: %d", static_cast<int8_t>(status));
+
+  if (status == TaskStatus::FAILED) {
+    return false;
+  } else if (status == TaskStatus::SUCCEEDED) {
+    // TODO(orduno): #443 check why task may report success while planner returns a path of 0 points
+    RCLCPP_DEBUG(this->get_logger(), "Got path, checking for possible collisions");
+    return isCollisionFree(path) && isWithinTolerance(robot_position, goal, path);
+  }
+
+  return false;
+}
+
+TaskStatus PlannerTester::createPlan(
+  const ComputePathToPoseCommand & goal,
+  ComputePathToPoseResult & path)
+{
+  // Update the costmap of the planner to the set data
+  planner_tester_->setCostmap(costmap_.get());
+
+  // Call planning algorithm
+  if (planner_tester_->createPath(goal, path)) {
+    return TaskStatus::SUCCEEDED;
+  }
+
+  return TaskStatus::FAILED;
+}
+
+bool PlannerTester::isPathValid(nav_msgs::msg::Path & path)
+{
+  planner_tester_->setCostmap(costmap_.get());
+  // create a fake service request
+  auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
+  request->path = path;
+  auto result = path_valid_client_->async_send_request(request);
+
+  RCLCPP_INFO(this->get_logger(), "Waiting for service complete");
+  if (rclcpp::spin_until_future_complete(
+      this->planner_tester_, result,
+      std::chrono::milliseconds(100)) ==
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    return result.get()->is_valid;
+  } else {
+    RCLCPP_INFO(get_logger(), "Failed to call is_path_valid service");
+    return false;
+  }
+}
+
+bool PlannerTester::isCollisionFree(const ComputePathToPoseResult & path)
+{
+  // At each point of the path, check if the corresponding cell is free
+
+  // TODO(orduno): #443 for now we are assuming the robot is the size of a single cell
+  //               costmap/world_model has consider the robot footprint
+
+  // TODO(orduno): #443 Tweak criteria for defining if a path goes into obstacles.
+  //               Current navfn planner will sometimes produce paths that cut corners
+  //               i.e. some points are around the corner are actually inside the obstacle
+
+  bool collisionFree = true;
+
+  for (auto pose : path.poses) {
+    collisionFree = costmap_->is_free(
+      static_cast<unsigned int>(std::round(pose.pose.position.x)),
+      static_cast<unsigned int>(std::round(pose.pose.position.y)));
+
+    if (!collisionFree) {
+      RCLCPP_WARN(
+        this->get_logger(), "Path has collision at (%.2f, %.2f)",
+        pose.pose.position.x, pose.pose.position.y);
+      printPath(path);
+      return false;
+    }
+  }
+
+  RCLCPP_DEBUG(this->get_logger(), "Path has no collisions");
+  return true;
+}
+
+bool PlannerTester::isWithinTolerance(
+  const geometry_msgs::msg::Point & robot_position,
+  const ComputePathToPoseCommand & goal,
+  const ComputePathToPoseResult & path) const
+{
+  return isWithinTolerance(
+    robot_position, goal, path, 0.0, ComputePathToPoseResult());
+}
+
+bool PlannerTester::isWithinTolerance(
+  const geometry_msgs::msg::Point & robot_position,
+  const ComputePathToPoseCommand & goal,
+  const ComputePathToPoseResult & path,
+  const double /*deviationTolerance*/,
+  const ComputePathToPoseResult & /*reference_path*/) const
+{
+  // TODO(orduno) #443 Work in progress, for now we only check that the path start matches the
+  //              robot start location and that the path end matches the goal.
+
+  auto path_start = path.poses[0];
+  auto path_end = path.poses.end()[-1];
+
+  if (
+    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");
+
+    return true;
+  }
+  RCLCPP_WARN(this->get_logger(), "Path deviates from requested start and end points");
+
+  RCLCPP_DEBUG(
+    this->get_logger(), "Requested path starts at (%.2f, %.2f) and ends at (%.2f, %.2f)",
+    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.pose.position.x, path_start.pose.position.y,
+    path_end.pose.position.x, path_end.pose.position.y);
+
+  return false;
+}
+
+void PlannerTester::printPath(const ComputePathToPoseResult & path) const
+{
+  auto index = 0;
+  auto ss = std::stringstream{};
+
+  for (auto pose : path.poses) {
+    ss << "   point #" << index << " with" <<
+      " x: " << std::setprecision(3) << pose.pose.position.x <<
+      " y: " << std::setprecision(3) << pose.pose.position.y << '\n';
+    ++index;
+  }
+
+  RCLCPP_INFO(get_logger(), ss.str().c_str());
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/planning/planner_tester.hpp

@@ -0,0 +1,241 @@
+// 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. Reserved.
+
+#ifndef PLANNING__PLANNER_TESTER_HPP_
+#define PLANNING__PLANNER_TESTER_HPP_
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <thread>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_msgs/srv/get_costmap.hpp"
+#include "nav2_msgs/srv/is_path_valid.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+#include "nav2_util/costmap.hpp"
+#include "nav2_util/node_thread.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+#include "nav2_planner/planner_server.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+
+namespace nav2_system_tests
+{
+
+class NavFnPlannerTester : public nav2_planner::PlannerServer
+{
+public:
+  NavFnPlannerTester()
+  : PlannerServer()
+  {
+  }
+
+  void printCostmap()
+  {
+    // print costmap for debug
+    for (size_t i = 0; i != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY(); i++) {
+      if (i % costmap_->getSizeInCellsX() == 0) {
+        std::cout << "" << std::endl;
+      }
+      std::cout << costmap_ros_->getCostmap()->getCharMap()[i] << " ";
+    }
+    std::cout << "" << std::endl;
+  }
+
+  void setCostmap(nav2_util::Costmap * costmap)
+  {
+    std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(
+      *(costmap_ros_->getCostmap()->getMutex()));
+
+    nav2_msgs::msg::CostmapMetaData prop;
+    nav2_msgs::msg::Costmap cm = costmap->get_costmap(prop);
+    prop = cm.metadata;
+    costmap_ros_->getCostmap()->resizeMap(
+      prop.size_x, prop.size_y,
+      prop.resolution, prop.origin.position.x, prop.origin.position.x);
+    // Volatile prevents compiler from treating costmap_ptr as unused or changing its address
+    volatile unsigned char * costmap_ptr = costmap_ros_->getCostmap()->getCharMap();
+    delete[] costmap_ptr;
+    costmap_ptr = new unsigned char[prop.size_x * prop.size_y];
+    std::copy(cm.data.begin(), cm.data.end(), costmap_ptr);
+  }
+
+  bool createPath(
+    const geometry_msgs::msg::PoseStamped & goal,
+    nav_msgs::msg::Path & path)
+  {
+    geometry_msgs::msg::PoseStamped start;
+    if (!nav2_util::getCurrentPose(start, *tf_, "map", "base_link", 0.1)) {
+      return false;
+    }
+    try {
+      path = planners_["GridBased"]->createPlan(start, goal);
+      // The situation when createPlan() did not throw any exception
+      // does not guarantee that plan was created correctly.
+      // So it should be checked additionally that path is correct.
+      if (!path.poses.size()) {
+        return false;
+      }
+    } catch (...) {
+      return false;
+    }
+    return true;
+  }
+
+  void onCleanup(const rclcpp_lifecycle::State & state)
+  {
+    on_cleanup(state);
+  }
+
+  void onActivate(const rclcpp_lifecycle::State & state)
+  {
+    on_activate(state);
+  }
+
+  void onDeactivate(const rclcpp_lifecycle::State & state)
+  {
+    on_deactivate(state);
+  }
+
+  void onConfigure(const rclcpp_lifecycle::State & state)
+  {
+    on_configure(state);
+  }
+};
+
+enum class TaskStatus : int8_t
+{
+  SUCCEEDED = 1,
+  FAILED = 2,
+  RUNNING = 3,
+};
+
+class PlannerTester : public rclcpp::Node
+{
+public:
+  using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
+  using ComputePathToPoseResult = nav_msgs::msg::Path;
+
+  PlannerTester();
+  ~PlannerTester();
+
+  // Activate the tester before running tests
+  void activate();
+  void deactivate();
+
+  // Loads the provided map and and generates a costmap from it.
+  void loadDefaultMap();
+
+  // Alternatively, use a preloaded 10x10 costmap
+  void loadSimpleCostmap(const nav2_util::TestCostmap & testCostmapType);
+
+  // Runs a single test with default poses depending on the loaded map
+  // Success criteria is a collision free path and a deviation to a
+  // reference path smaller than a tolerance.
+  bool defaultPlannerTest(
+    ComputePathToPoseResult & path,
+    const double deviation_tolerance = 1.0);
+
+
+  // Runs multiple tests with random initial and goal poses
+  bool defaultPlannerRandomTests(
+    const unsigned int number_tests,
+    const float acceptable_fail_ratio);
+
+  bool isPathValid(nav_msgs::msg::Path & path);
+
+private:
+  void setCostmap();
+
+  TaskStatus createPlan(
+    const ComputePathToPoseCommand & goal,
+    ComputePathToPoseResult & path
+  );
+
+  bool is_active_;
+  bool map_set_;
+  bool costmap_set_;
+  bool using_fake_costmap_;
+
+  // Parameters of the costmap
+  bool trinary_costmap_;
+  bool track_unknown_space_;
+  int lethal_threshold_;
+  int unknown_cost_value_;
+  nav2_util::TestCostmap testCostmapType_;
+
+  // The static map
+  std::shared_ptr<nav_msgs::msg::OccupancyGrid> map_;
+
+  // The costmap representation of the static map
+  std::unique_ptr<nav2_util::Costmap> costmap_;
+
+  // The global planner
+  std::shared_ptr<NavFnPlannerTester> planner_tester_;
+
+  // The is path valid client
+  rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr path_valid_client_;
+
+  // A thread for spinning the ROS node
+  std::unique_ptr<nav2_util::NodeThread> spin_thread_;
+
+  // The tester must provide the robot pose through a transform
+  std::unique_ptr<geometry_msgs::msg::TransformStamped> base_transform_;
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  rclcpp::TimerBase::SharedPtr transform_timer_;
+  void publishRobotTransform();
+  void startRobotTransform();
+  void updateRobotPosition(const geometry_msgs::msg::Point & position);
+
+  // Occupancy grid publisher for visualization
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
+  rclcpp::TimerBase::SharedPtr map_timer_;
+  rclcpp::WallRate map_publish_rate_;
+  void mapCallback();
+
+  // Executes a test run with the provided end points.
+  // Success criteria is a collision free path.
+  // TODO(orduno): #443 Assuming a robot the size of a costmap cell
+  bool plannerTest(
+    const geometry_msgs::msg::Point & robot_position,
+    const ComputePathToPoseCommand & goal,
+    ComputePathToPoseResult & path);
+
+  bool isCollisionFree(const ComputePathToPoseResult & path);
+
+  bool isWithinTolerance(
+    const geometry_msgs::msg::Point & robot_position,
+    const ComputePathToPoseCommand & goal,
+    const ComputePathToPoseResult & path) const;
+
+  bool isWithinTolerance(
+    const geometry_msgs::msg::Point & robot_position,
+    const ComputePathToPoseCommand & goal,
+    const ComputePathToPoseResult & path,
+    const double deviationTolerance,
+    const ComputePathToPoseResult & reference_path) const;
+
+  void printPath(const ComputePathToPoseResult & path) const;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // PLANNING__PLANNER_TESTER_HPP_

navigation2/nav2_system_tests/src/planning/test_planner_costmaps_launch.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+
+# 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.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    ld = LaunchDescription([])
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable, '--ros-args -p use_sim_time:=True'],
+        name='test_planner_costmaps_node',
+        output='screen'
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp

@@ -0,0 +1,67 @@
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "planner_tester.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::PlannerTester;
+using nav2_util::TestCostmap;
+
+using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
+using ComputePathToPoseResult = nav_msgs::msg::Path;
+
+TEST(testSimpleCostmaps, testSimpleCostmaps)
+{
+  auto obj = std::make_shared<PlannerTester>();
+
+  std::vector<TestCostmap> costmaps = {
+    TestCostmap::open_space,
+    TestCostmap::bounded,
+    TestCostmap::top_left_obstacle,
+    TestCostmap::bottom_left_obstacle,
+    TestCostmap::maze1,
+    TestCostmap::maze2
+  };
+
+  ComputePathToPoseResult result;
+
+  obj->activate();
+
+  for (auto costmap : costmaps) {
+    obj->loadSimpleCostmap(costmap);
+    EXPECT_EQ(true, obj->defaultPlannerTest(result));
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp

@@ -0,0 +1,75 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+
+#include "nav2_msgs/srv/is_path_valid.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "planner_tester.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+
+using nav2_system_tests::PlannerTester;
+using nav2_util::TestCostmap;
+
+TEST(testIsPathValid, testIsPathValid)
+{
+  auto planner_tester = std::make_shared<PlannerTester>();
+  planner_tester->activate();
+  planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle);
+
+  nav_msgs::msg::Path path;
+
+  // empty path
+  bool is_path_valid = planner_tester->isPathValid(path);
+  EXPECT_FALSE(is_path_valid);
+
+  // invalid path
+  for (float i = 0; i < 10; i += 1.0) {
+    for (float j = 0; j < 10; j += 1.0) {
+      geometry_msgs::msg::PoseStamped pose;
+      pose.pose.position.x = i;
+      pose.pose.position.y = j;
+      path.poses.push_back(pose);
+    }
+  }
+  is_path_valid = planner_tester->isPathValid(path);
+  EXPECT_FALSE(is_path_valid);
+
+  // valid path
+  path.poses.clear();
+  for (float i = 0; i < 10; i += 1.0) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = 1.0;
+    pose.pose.position.y = i;
+    path.poses.push_back(pose);
+  }
+  is_path_valid = planner_tester->isPathValid(path);
+  EXPECT_TRUE(is_path_valid);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  return all_successful;
+}

navigation2/nav2_system_tests/src/planning/test_planner_plugins.cpp

@@ -0,0 +1,301 @@
+// Copyright (c) 2020 Samsung Research
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "planner_tester.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::PlannerTester;
+using nav2_util::TestCostmap;
+
+using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
+using ComputePathToPoseResult = nav_msgs::msg::Path;
+
+void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/)
+{
+}
+
+void testSmallPathValidityAndOrientation(std::string plugin, double length)
+{
+  auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
+  rclcpp_lifecycle::State state;
+  obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin));
+  obj->declare_parameter(
+    "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(false));
+  obj->onConfigure(state);
+
+  geometry_msgs::msg::PoseStamped start;
+  geometry_msgs::msg::PoseStamped goal;
+
+  start.pose.position.x = 0.5;
+  start.pose.position.y = 0.5;
+  start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);
+  start.header.frame_id = "map";
+
+  goal.pose.position.x = 0.5;
+  goal.pose.position.y = start.pose.position.y + length;
+  goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI);
+  goal.header.frame_id = "map";
+
+  // Test without use_final_approach_orientation
+  // expecting end path pose orientation to be equal to goal orientation
+  auto path = obj->getPlan(start, goal, "GridBased");
+  EXPECT_GT((int)path.poses.size(), 0);
+  EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01);
+  // obj->onCleanup(state);
+  obj.reset();
+}
+
+void testSmallPathValidityAndNoOrientation(std::string plugin, double length)
+{
+  auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
+  rclcpp_lifecycle::State state;
+  obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin));
+
+  // Test WITH use_final_approach_orientation
+  // expecting end path pose orientation to be equal to approach orientation
+  // which in the one pose corner case should be the start pose orientation
+  obj->declare_parameter(
+    "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(true));
+  obj->set_parameter(rclcpp::Parameter("GridBased.use_final_approach_orientation", true));
+  obj->onConfigure(state);
+
+  geometry_msgs::msg::PoseStamped start;
+  geometry_msgs::msg::PoseStamped goal;
+
+  start.pose.position.x = 0.5;
+  start.pose.position.y = 0.5;
+  start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);
+  start.header.frame_id = "map";
+
+  goal.pose.position.x = 0.5;
+  goal.pose.position.y = start.pose.position.y + length;
+  goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI);
+  goal.header.frame_id = "map";
+
+  auto path = obj->getPlan(start, goal, "GridBased");
+  EXPECT_GT((int)path.poses.size(), 0);
+
+  int path_size = path.poses.size();
+  if (path_size == 1) {
+    EXPECT_NEAR(
+      tf2::getYaw(path.poses.back().pose.orientation),
+      tf2::getYaw(start.pose.orientation),
+      0.01);
+  } else {
+    double dx = path.poses.back().pose.position.x - path.poses.front().pose.position.x;
+    double dy = path.poses.back().pose.position.y - path.poses.front().pose.position.y;
+    EXPECT_NEAR(
+      tf2::getYaw(path.poses.back().pose.orientation),
+      atan2(dy, dx),
+      0.01);
+  }
+  // obj->onCleanup(state);
+  obj.reset();
+}
+
+TEST(testPluginMap, Failures)
+{
+  auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
+  rclcpp_lifecycle::State state;
+  obj->set_parameter(rclcpp::Parameter("expected_planner_frequency", 100000.0));
+  obj->onConfigure(state);
+  obj->create_subscription<nav_msgs::msg::Path>(
+    "plan", rclcpp::SystemDefaultsQoS(), callback);
+
+  geometry_msgs::msg::PoseStamped start;
+  geometry_msgs::msg::PoseStamped goal;
+  std::string plugin_fake = "fake";
+  std::string plugin_none = "";
+  auto path = obj->getPlan(start, goal, plugin_none);
+  EXPECT_EQ(path.header.frame_id, std::string("map"));
+
+  path = obj->getPlan(start, goal, plugin_fake);
+  EXPECT_EQ(path.poses.size(), 0ul);
+
+  obj->onCleanup(state);
+}
+
+TEST(testPluginMap, Smac2dEqualStartGoal)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0);
+}
+
+TEST(testPluginMap, Smac2dEqualStartGoalN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.0);
+}
+
+TEST(testPluginMap, Smac2dVerySmallPath)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.00001);
+}
+
+TEST(testPluginMap, Smac2dVerySmallPathN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.00001);
+}
+
+TEST(testPluginMap, Smac2dBelowCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.09);
+}
+
+TEST(testPluginMap, Smac2dBelowCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.09);
+}
+
+TEST(testPluginMap, Smac2dJustAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.102);
+}
+
+TEST(testPluginMap, Smac2dJustAboveCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.102);
+}
+
+TEST(testPluginMap, Smac2dAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 1.5);
+}
+
+TEST(testPluginMap, Smac2dAboveCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 1.5);
+}
+
+TEST(testPluginMap, NavFnEqualStartGoal)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.0);
+}
+
+TEST(testPluginMap, NavFnEqualStartGoalN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.0);
+}
+
+TEST(testPluginMap, NavFnVerySmallPath)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.00001);
+}
+
+TEST(testPluginMap, NavFnVerySmallPathN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.00001);
+}
+
+TEST(testPluginMap, NavFnBelowCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.09);
+}
+
+TEST(testPluginMap, NavFnBelowCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.09);
+}
+
+TEST(testPluginMap, NavFnJustAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.102);
+}
+
+TEST(testPluginMap, NavFnJustAboveCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.102);
+}
+
+TEST(testPluginMap, NavFnAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 1.5);
+}
+
+TEST(testPluginMap, NavFnAboveCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 1.5);
+}
+
+TEST(testPluginMap, ThetaStarEqualStartGoal)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.0);
+}
+
+TEST(testPluginMap, ThetaStarEqualStartGoalN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.0);
+}
+
+TEST(testPluginMap, ThetaStarVerySmallPath)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.00001);
+}
+
+TEST(testPluginMap, ThetaStarVerySmallPathN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.00001);
+}
+
+TEST(testPluginMap, ThetaStarBelowCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.09);
+}
+
+TEST(testPluginMap, ThetaStarBelowCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.09);
+}
+
+TEST(testPluginMap, ThetaStarJustAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.102);
+}
+
+TEST(testPluginMap, ThetaStarJustAboveCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.102);
+}
+
+TEST(testPluginMap, ThetaStarAboveCostmapResolution)
+{
+  testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5);
+}
+
+TEST(testPluginMap, ThetaStarAboveCostmapResolutionN)
+{
+  testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_system_tests/src/planning/test_planner_random_launch.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+
+# 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.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    ld = LaunchDescription([])
+
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable, '--ros-args -p use_sim_time:=True'],
+        name='test_planner_random_node',
+        output='screen'
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/planning/test_planner_random_node.cpp

@@ -0,0 +1,62 @@
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+#include <iostream>
+
+#include "rclcpp/rclcpp.hpp"
+#include "planner_tester.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::PlannerTester;
+using nav2_util::TestCostmap;
+
+using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
+using ComputePathToPoseResult = nav_msgs::msg::Path;
+
+TEST(testWithHundredRandomEndPoints, testWithHundredRandomEndPoints)
+{
+  auto obj = std::make_shared<PlannerTester>();
+  obj->activate();
+  obj->loadDefaultMap();
+
+  bool success = false;
+  int num_tries = 3;
+  for (int i = 0; i != num_tries; i++) {
+    success = success || obj->defaultPlannerRandomTests(100, 0.1);
+    if (success) {
+      break;
+    }
+  }
+
+  EXPECT_EQ(true, success);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

navigation2/nav2_system_tests/src/system/CMakeLists.txt

@@ -0,0 +1,118 @@
+ament_add_test(test_bt_navigator
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  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
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+    TESTER=nav_to_pose_tester_node.py
+    ASTAR=True
+    CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+    PLANNER=nav2_navfn_planner/NavfnPlanner
+)
+
+ament_add_test(test_bt_navigator_with_wrong_init_pose
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  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
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+    TESTER=nav_to_pose_tester_node.py
+    ASTAR=True
+    CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+    PLANNER=nav2_navfn_planner/NavfnPlanner
+)
+
+ament_add_test(test_bt_navigator_with_dijkstra
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  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
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+    TESTER=nav_to_pose_tester_node.py
+    ASTAR=False
+    CONTROLLER=dwb_core::DWBLocalPlanner
+    PLANNER=nav2_navfn_planner/NavfnPlanner
+)
+
+ament_add_test(test_bt_navigator_2
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  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
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+    TESTER=nav_to_pose_tester_node.py
+    ASTAR=False
+    CONTROLLER=dwb_core::DWBLocalPlanner
+    PLANNER=nav2_navfn_planner/NavfnPlanner
+)
+
+ament_add_test(test_dynamic_obstacle
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  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_obstacle.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+    TESTER=nav_to_pose_tester_node.py
+    ASTAR=False
+    CONTROLLER=dwb_core::DWBLocalPlanner
+    PLANNER=nav2_navfn_planner/NavfnPlanner
+)
+
+ament_add_test(test_nav_through_poses
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  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_obstacle.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
+    TESTER=nav_through_poses_tester_node.py
+    ASTAR=False
+    CONTROLLER=dwb_core::DWBLocalPlanner
+    PLANNER=nav2_navfn_planner/NavfnPlanner
+)
+
+# ament_add_test(test_multi_robot
+#   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+#   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_multi_robot_launch.py"
+#   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+#   TIMEOUT 180
+#   ENV
+#     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+#     TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+#     TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/world_only.model
+#     TEST_URDF=${PROJECT_SOURCE_DIR}/urdf/turtlebot3_waffle.urdf
+#     TEST_SDF=${PROJECT_SOURCE_DIR}/models/turtlebot3_waffle/model.sdf
+#     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+#     CONTROLLER=dwb_core::DWBLocalPlanner
+#     PLANNER=nav2_navfn_planner/NavfnPlanner
+#     TESTER=nav_to_pose_tester_node.py
+# )

navigation2/nav2_system_tests/src/system/README.md

@@ -0,0 +1,38 @@
+# Nav2 System Tests
+
+This is a 'top level' system test which will use Gazebo to simulate a Robot moving from an known initial starting position to a goal pose. 
+
+## To run the test
+First, you must build Nav2 including this package:
+```
+colcon build --symlink-install
+```
+Then you can run all the system tests:
+```
+colcon test --packages-select nav2_system_tests
+```
+Output results will go to the screen, and will be logged to the "log/latest_test/nav2_system_tests/" path relative to where colcon test was run.
+
+To run just the bt_navigator test:
+```
+cd build/nav2_system_tests
+ctest -V bt_navigator$
+```
+
+To loop over the bt_navigator test, a script has been provided:
+```
+nav2_system_tests/scripts/ctest_loop.bash -c <# loops> -o <path/to/summary/filename.txt> -l <path/to/store/failing/logfiles.log> -d <dds to use>
+```
+
+Example (loop 100 times using fastrtps):
+```
+nav2_system_tests/scripts/getopt_ctest_loop.bash -c 100 -o /home/robot/data/results.txt -l /home/robot/data/results.log -d rmw_fastrtps_cpp
+```
+
+## Notes: (updated Aug 2019)
+ * This currently uses a turtlebot3 robot model, world and map.
+ * The test normally takes 1-2 minutes to run, with a timeout of 2 minutes
+
+## Future Work
+ * Add additional goal poses if the first one successfully passes
+ * Remove the dependency on the turtlebot3 model and map by adding a simple / dummy robot and creating a world and map

navigation2/nav2_system_tests/src/system/nav_through_poses_tester_node.py

@@ -0,0 +1,344 @@
+#! /usr/bin/env python3
+# Copyright 2018 Intel Corporation.
+# Copyright 2020 Florian Gramss
+#
+# 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.
+
+import argparse
+import sys
+import time
+
+from typing import Optional
+
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from lifecycle_msgs.srv import GetState
+from nav2_msgs.action import NavigateThroughPoses
+from nav2_msgs.srv import ManageLifecycleNodes
+
+import rclpy
+
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+
+
+class NavTester(Node):
+
+    def __init__(
+        self,
+        initial_pose: Pose,
+        goal_pose: Pose,
+        namespace: str = ''
+    ):
+        super().__init__(node_name='nav2_tester', namespace=namespace)
+        self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
+                                                      'initialpose', 10)
+
+        pose_qos = QoSProfile(
+          durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+          reliability=QoSReliabilityPolicy.RELIABLE,
+          history=QoSHistoryPolicy.KEEP_LAST,
+          depth=1)
+
+        self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
+                                                       'amcl_pose', self.poseCallback, pose_qos)
+        self.initial_pose_received = False
+        self.initial_pose = initial_pose
+        self.goal_pose = goal_pose
+        self.action_client = ActionClient(self, NavigateThroughPoses, 'navigate_through_poses')
+
+    def info_msg(self, msg: str):
+        self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
+
+    def warn_msg(self, msg: str):
+        self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
+
+    def error_msg(self, msg: str):
+        self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
+
+    def setInitialPose(self):
+        msg = PoseWithCovarianceStamped()
+        msg.pose.pose = self.initial_pose
+        msg.header.frame_id = 'map'
+        self.info_msg('Publishing Initial Pose')
+        self.initial_pose_pub.publish(msg)
+        self.currentPose = self.initial_pose
+
+    def getStampedPoseMsg(self, pose: Pose):
+        msg = PoseStamped()
+        msg.header.frame_id = 'map'
+        msg.pose = pose
+        return msg
+
+    def runNavigateAction(self, goal_pose: Optional[Pose] = None):
+        # Sends a `NavToPose` action request and waits for completion
+        self.info_msg("Waiting for 'NavigateThroughPoses' action server")
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'NavigateThroughPoses' action server not available, waiting...")
+
+        self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
+        goal_msg = NavigateThroughPoses.Goal()
+        goal_msg.poses = [self.getStampedPoseMsg(self.goal_pose),
+                          self.getStampedPoseMsg(self.goal_pose)]
+
+        self.info_msg('Sending goal request...')
+        send_goal_future = self.action_client.send_goal_async(goal_msg)
+
+        rclpy.spin_until_future_complete(self, send_goal_future)
+        goal_handle = send_goal_future.result()
+
+        if not goal_handle.accepted:
+            self.error_msg('Goal rejected')
+            return False
+
+        self.info_msg('Goal accepted')
+        get_result_future = goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'NavigateToPose' action to complete")
+        rclpy.spin_until_future_complete(self, get_result_future)
+        status = get_result_future.result().status
+        if status != GoalStatus.STATUS_SUCCEEDED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+
+        self.info_msg('Goal succeeded!')
+        return True
+
+    def runFakeNavigateAction(self):
+        # Sends a `NavToPose` action request and waits for completion
+        self.info_msg("Waiting for 'NavigateThroughPoses' action server")
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'NavigateThroughPoses' action server not available, waiting...")
+
+        goal_msg = NavigateThroughPoses.Goal()
+
+        self.info_msg('Sending goal request...')
+        send_goal_future = self.action_client.send_goal_async(goal_msg)
+
+        rclpy.spin_until_future_complete(self, send_goal_future)
+        goal_handle = send_goal_future.result()
+
+        if not goal_handle.accepted:
+            self.error_msg('Goal rejected')
+            return False
+
+        self.info_msg('Goal accepted')
+        get_result_future = goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'NavigateToPose' action to complete")
+        rclpy.spin_until_future_complete(self, get_result_future)
+        status = get_result_future.result().status
+        if status != GoalStatus.STATUS_SUCCEEDED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+
+        self.info_msg('Goal succeeded!')
+        return True
+
+    def runNavigatePreemptionAction(self, block):
+        # Sends a `NavToPose` action request and waits for completion
+        self.info_msg("Waiting for 'NavigateThroughPoses' action server")
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'NavigateThroughPoses' action server not available, waiting...")
+
+        goal_msg = NavigateThroughPoses.Goal()
+        goal_msg.poses = [self.getStampedPoseMsg(self.initial_pose)]
+
+        self.info_msg('Sending goal request...')
+        send_goal_future = self.action_client.send_goal_async(goal_msg)
+
+        rclpy.spin_until_future_complete(self, send_goal_future)
+        goal_handle = send_goal_future.result()
+
+        if not goal_handle.accepted:
+            self.error_msg('Goal rejected')
+            return False
+
+        if not block:
+            return True
+
+        self.info_msg('Goal accepted')
+        get_result_future = goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'NavigateToPose' action to complete")
+        rclpy.spin_until_future_complete(self, get_result_future)
+        status = get_result_future.result().status
+        if status != GoalStatus.STATUS_SUCCEEDED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+
+        self.info_msg('Goal succeeded!')
+        return True
+
+    def poseCallback(self, msg):
+        self.info_msg('Received amcl_pose')
+        self.current_pose = msg.pose.pose
+        self.initial_pose_received = True
+
+    def wait_for_node_active(self, node_name: str):
+        # Waits for the node within the tester namespace to become active
+        self.info_msg(f'Waiting for {node_name} to become active')
+        node_service = f'{node_name}/get_state'
+        state_client = self.create_client(GetState, node_service)
+        while not state_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{node_service} service not available, waiting...')
+        req = GetState.Request()  # empty request
+        state = 'UNKNOWN'
+        while (state != 'active'):
+            self.info_msg(f'Getting {node_name} state...')
+            future = state_client.call_async(req)
+            rclpy.spin_until_future_complete(self, future)
+            if future.result() is not None:
+                state = future.result().current_state.label
+                self.info_msg(f'Result of get_state: {state}')
+            else:
+                self.error_msg(f'Exception while calling service: {future.exception()!r}')
+            time.sleep(5)
+
+    def shutdown(self):
+        self.info_msg('Shutting down')
+        self.action_client.destroy()
+
+        transition_service = 'lifecycle_manager_navigation/manage_nodes'
+        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            self.info_msg('Shutting down navigation lifecycle manager...')
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+            self.info_msg('Shutting down navigation lifecycle manager complete.')
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+        transition_service = 'lifecycle_manager_localization/manage_nodes'
+        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            self.info_msg('Shutting down localization lifecycle manager...')
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+            self.info_msg('Shutting down localization lifecycle manager complete')
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+
+    def wait_for_initial_pose(self):
+        self.initial_pose_received = False
+        while not self.initial_pose_received:
+            self.info_msg('Setting initial pose')
+            self.setInitialPose()
+            self.info_msg('Waiting for amcl_pose to be received')
+            rclpy.spin_once(self, timeout_sec=1)
+
+
+def run_all_tests(robot_tester):
+    # set transforms to use_sim_time
+    result = True
+    if (result):
+        robot_tester.wait_for_node_active('amcl')
+        robot_tester.wait_for_initial_pose()
+        robot_tester.wait_for_node_active('bt_navigator')
+        result = robot_tester.runNavigateAction()
+        # Test empty navigation request
+        result = result and not robot_tester.runFakeNavigateAction()
+        # Test preempting NHP with the same goal
+        result = result and robot_tester.runNavigatePreemptionAction(False)
+        result = result and robot_tester.runNavigatePreemptionAction(True)
+
+    # Add more tests here if desired
+
+    if (result):
+        robot_tester.info_msg('Test PASSED')
+    else:
+        robot_tester.error_msg('Test FAILED')
+
+    return result
+
+
+def fwd_pose(x=0.0, y=0.0, z=0.01):
+    initial_pose = Pose()
+    initial_pose.position.x = x
+    initial_pose.position.y = y
+    initial_pose.position.z = z
+    initial_pose.orientation.x = 0.0
+    initial_pose.orientation.y = 0.0
+    initial_pose.orientation.z = 0.0
+    initial_pose.orientation.w = 1.0
+    return initial_pose
+
+
+def get_testers(args):
+    testers = []
+
+    init_x, init_y, final_x, final_y = args.robot[0]
+    tester = NavTester(
+        initial_pose=fwd_pose(float(init_x), float(init_y)),
+        goal_pose=fwd_pose(float(final_x), float(final_y)))
+    tester.info_msg(
+        'Starting tester, robot going from ' + init_x + ', ' + init_y +
+        ' to ' + final_x + ', ' + final_y + '.')
+    testers.append(tester)
+    return testers
+
+
+def main(argv=sys.argv[1:]):
+    # The robot(s) positions from the input arguments
+    parser = argparse.ArgumentParser(description='System-level navigation tester node')
+    group = parser.add_mutually_exclusive_group(required=True)
+    group.add_argument('-r', '--robot', action='append', nargs=4,
+                       metavar=('init_x', 'init_y', 'final_x', 'final_y'),
+                       help='The robot starting and final positions.')
+
+    args, unknown = parser.parse_known_args()
+
+    rclpy.init()
+
+    # Create testers for each robot
+    testers = get_testers(args)
+
+    # wait a few seconds to make sure entire stacks are up
+    time.sleep(10)
+
+    for tester in testers:
+        passed = run_all_tests(tester)
+        if not passed:
+            break
+
+    for tester in testers:
+        # stop and shutdown the nav stack to exit cleanly
+        tester.shutdown()
+
+    testers[0].info_msg('Done Shutting Down.')
+
+    if not passed:
+        testers[0].info_msg('Exiting failed')
+        exit(1)
+    else:
+        testers[0].info_msg('Exiting passed')
+        exit(0)
+
+
+if __name__ == '__main__':
+    main()

navigation2/nav2_system_tests/src/system/nav_to_pose_tester_node.py

@@ -0,0 +1,377 @@
+#! /usr/bin/env python3
+# Copyright 2018 Intel Corporation.
+# Copyright 2020 Florian Gramss
+#
+# 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.
+
+import argparse
+import math
+import sys
+import time
+
+from typing import Optional
+
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from lifecycle_msgs.srv import GetState
+from nav2_msgs.action import NavigateToPose
+from nav2_msgs.srv import ManageLifecycleNodes
+
+import rclpy
+
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+
+
+class NavTester(Node):
+
+    def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''):
+        super().__init__(node_name='nav2_tester', namespace=namespace)
+        self.initial_pose_pub = self.create_publisher(
+            PoseWithCovarianceStamped, 'initialpose', 10
+        )
+        self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
+
+        pose_qos = QoSProfile(
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+
+        self.model_pose_sub = self.create_subscription(
+            PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos
+        )
+        self.initial_pose_received = False
+        self.initial_pose = initial_pose
+        self.goal_pose = goal_pose
+        self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
+
+    def info_msg(self, msg: str):
+        self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
+
+    def warn_msg(self, msg: str):
+        self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
+
+    def error_msg(self, msg: str):
+        self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
+
+    def setInitialPose(self):
+        msg = PoseWithCovarianceStamped()
+        msg.pose.pose = self.initial_pose
+        msg.header.frame_id = 'map'
+        self.info_msg('Publishing Initial Pose')
+        self.initial_pose_pub.publish(msg)
+        self.currentPose = self.initial_pose
+
+    def getStampedPoseMsg(self, pose: Pose):
+        msg = PoseStamped()
+        msg.header.frame_id = 'map'
+        msg.pose = pose
+        return msg
+
+    def publishGoalPose(self, goal_pose: Optional[Pose] = None):
+        self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
+        self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
+
+    def runNavigateAction(self, goal_pose: Optional[Pose] = None):
+        # Sends a `NavToPose` action request and waits for completion
+        self.info_msg("Waiting for 'NavigateToPose' action server")
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'NavigateToPose' action server not available, waiting...")
+
+        self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
+        goal_msg = NavigateToPose.Goal()
+        goal_msg.pose = self.getStampedPoseMsg(self.goal_pose)
+
+        self.info_msg('Sending goal request...')
+        send_goal_future = self.action_client.send_goal_async(goal_msg)
+
+        rclpy.spin_until_future_complete(self, send_goal_future)
+        goal_handle = send_goal_future.result()
+
+        if not goal_handle.accepted:
+            self.error_msg('Goal rejected')
+            return False
+
+        self.info_msg('Goal accepted')
+        get_result_future = goal_handle.get_result_async()
+
+        future_return = True
+
+        self.info_msg("Waiting for 'NavigateToPose' action to complete")
+        rclpy.spin_until_future_complete(self, get_result_future)
+        status = get_result_future.result().status
+        if status != GoalStatus.STATUS_SUCCEEDED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+
+        if not future_return:
+            return False
+
+        self.info_msg('Goal succeeded!')
+        return True
+
+    def poseCallback(self, msg):
+        self.info_msg('Received amcl_pose')
+        self.current_pose = msg.pose.pose
+        self.initial_pose_received = True
+
+    def reachesGoal(self, timeout, distance):
+        goalReached = False
+        start_time = time.time()
+
+        while not goalReached:
+            rclpy.spin_once(self, timeout_sec=1)
+            if self.distanceFromGoal() < distance:
+                goalReached = True
+                self.info_msg('*** GOAL REACHED ***')
+                return True
+            elif timeout is not None:
+                if (time.time() - start_time) > timeout:
+                    self.error_msg('Robot timed out reaching its goal!')
+                    return False
+
+    def distanceFromGoal(self):
+        d_x = self.current_pose.position.x - self.goal_pose.position.x
+        d_y = self.current_pose.position.y - self.goal_pose.position.y
+        distance = math.sqrt(d_x * d_x + d_y * d_y)
+        self.info_msg(f'Distance from goal is: {distance}')
+        return distance
+
+    def wait_for_node_active(self, node_name: str):
+        # Waits for the node within the tester namespace to become active
+        self.info_msg(f'Waiting for {node_name} to become active')
+        node_service = f'{node_name}/get_state'
+        state_client = self.create_client(GetState, node_service)
+        while not state_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{node_service} service not available, waiting...')
+        req = GetState.Request()  # empty request
+        state = 'UNKNOWN'
+        while state != 'active':
+            self.info_msg(f'Getting {node_name} state...')
+            future = state_client.call_async(req)
+            rclpy.spin_until_future_complete(self, future)
+            if future.result() is not None:
+                state = future.result().current_state.label
+                self.info_msg(f'Result of get_state: {state}')
+            else:
+                self.error_msg(
+                    f'Exception while calling service: {future.exception()!r}'
+                )
+            time.sleep(5)
+
+    def shutdown(self):
+        self.info_msg('Shutting down')
+        self.action_client.destroy()
+
+        transition_service = 'lifecycle_manager_navigation/manage_nodes'
+        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            self.info_msg('Shutting down navigation lifecycle manager...')
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+            self.info_msg('Shutting down navigation lifecycle manager complete.')
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+        transition_service = 'lifecycle_manager_localization/manage_nodes'
+        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            self.info_msg('Shutting down localization lifecycle manager...')
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+            self.info_msg('Shutting down localization lifecycle manager complete')
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+
+    def wait_for_initial_pose(self):
+        self.initial_pose_received = False
+        while not self.initial_pose_received:
+            self.info_msg('Setting initial pose')
+            self.setInitialPose()
+            self.info_msg('Waiting for amcl_pose to be received')
+            rclpy.spin_once(self, timeout_sec=1)
+
+
+def test_RobotMovesToGoal(robot_tester):
+    robot_tester.info_msg('Setting goal pose')
+    robot_tester.publishGoalPose()
+    robot_tester.info_msg('Waiting 60 seconds for robot to reach goal')
+    return robot_tester.reachesGoal(timeout=60, distance=0.5)
+
+
+def run_all_tests(robot_tester):
+    # set transforms to use_sim_time
+    result = True
+    if result:
+        robot_tester.wait_for_node_active('amcl')
+        robot_tester.wait_for_initial_pose()
+        robot_tester.wait_for_node_active('bt_navigator')
+        result = robot_tester.runNavigateAction()
+
+    if result:
+        result = test_RobotMovesToGoal(robot_tester)
+
+    # Add more tests here if desired
+
+    if result:
+        robot_tester.info_msg('Test PASSED')
+    else:
+        robot_tester.error_msg('Test FAILED')
+
+    return result
+
+
+def fwd_pose(x=0.0, y=0.0, z=0.01):
+    initial_pose = Pose()
+    initial_pose.position.x = x
+    initial_pose.position.y = y
+    initial_pose.position.z = z
+    initial_pose.orientation.x = 0.0
+    initial_pose.orientation.y = 0.0
+    initial_pose.orientation.z = 0.0
+    initial_pose.orientation.w = 1.0
+    return initial_pose
+
+
+def get_testers(args):
+    testers = []
+
+    if args.robot:
+        # Requested tester for one robot
+        init_x, init_y, final_x, final_y = args.robot[0]
+        tester = NavTester(
+            initial_pose=fwd_pose(float(init_x), float(init_y)),
+            goal_pose=fwd_pose(float(final_x), float(final_y)),
+        )
+        tester.info_msg(
+            'Starting tester, robot going from '
+            + init_x
+            + ', '
+            + init_y
+            + ' to '
+            + final_x
+            + ', '
+            + final_y
+            + '.'
+        )
+        testers.append(tester)
+        return testers
+
+    # Requested tester for multiple robots
+    for robot in args.robots:
+        namespace, init_x, init_y, final_x, final_y = robot
+        tester = NavTester(
+            namespace=namespace,
+            initial_pose=fwd_pose(float(init_x), float(init_y)),
+            goal_pose=fwd_pose(float(final_x), float(final_y)),
+        )
+        tester.info_msg(
+            'Starting tester for '
+            + namespace
+            + ' going from '
+            + init_x
+            + ', '
+            + init_y
+            + ' to '
+            + final_x
+            + ', '
+            + final_y
+        )
+        testers.append(tester)
+    return testers
+
+
+def check_args(expect_failure: str):
+    # Check if --expect_failure is True or False
+    if expect_failure != 'True' and expect_failure != 'False':
+        print(
+            '\033[1;37;41m' + ' -e flag must be set to True or False only. ' + '\033[0m'
+        )
+        exit(1)
+    else:
+        return eval(expect_failure)
+
+
+def main(argv=sys.argv[1:]):
+    # The robot(s) positions from the input arguments
+    parser = argparse.ArgumentParser(description='System-level navigation tester node')
+    parser.add_argument('-e', '--expect_failure')
+    group = parser.add_mutually_exclusive_group(required=True)
+    group.add_argument(
+        '-r',
+        '--robot',
+        action='append',
+        nargs=4,
+        metavar=('init_x', 'init_y', 'final_x', 'final_y'),
+        help='The robot starting and final positions.',
+    )
+    group.add_argument(
+        '-rs',
+        '--robots',
+        action='append',
+        nargs=5,
+        metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'),
+        help="The robot's namespace and starting and final positions. "
+        + 'Repeating the argument for multiple robots is supported.',
+    )
+
+    args, unknown = parser.parse_known_args()
+
+    expect_failure = check_args(args.expect_failure)
+
+    rclpy.init()
+
+    # Create testers for each robot
+    testers = get_testers(args)
+
+    # wait a few seconds to make sure entire stacks are up
+    time.sleep(10)
+
+    for tester in testers:
+        passed = run_all_tests(tester)
+        if passed != expect_failure:
+            break
+
+    for tester in testers:
+        # stop and shutdown the nav stack to exit cleanly
+        tester.shutdown()
+
+    testers[0].info_msg('Done Shutting Down.')
+
+    if passed != expect_failure:
+        testers[0].info_msg('Exiting failed')
+        exit(1)
+    else:
+        testers[0].info_msg('Exiting passed')
+        exit(0)
+
+
+if __name__ == '__main__':
+    main()

navigation2/nav2_system_tests/src/system/test_multi_robot_launch.py

@@ -0,0 +1,147 @@
+#!/usr/bin/env python3
+
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription, LaunchService
+from launch.actions import (ExecuteProcess, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import TextSubstitution
+from launch_ros.actions import Node, PushRosNamespace
+
+from launch_testing.legacy import LaunchTestService
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+    urdf = os.getenv('TEST_URDF')
+    sdf = os.getenv('TEST_SDF')
+
+    bt_xml_file = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                               'behavior_trees',
+                               os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    robot1_params_file = os.path.join(bringup_dir,  # noqa: F841
+                                      'params/nav2_multirobot_params_1.yaml')
+    robot2_params_file = os.path.join(bringup_dir,  # noqa: F841
+                                      'params/nav2_multirobot_params_2.yaml')
+
+    # Names and poses of the robots
+    robots = [
+        {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01},
+        {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}]
+
+    # Launch Gazebo server for simulation
+    start_gazebo_cmd = ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '-s', 'libgazebo_ros_factory.so', '--minimal_comms', world],
+            output='screen')
+
+    # Define commands for spawing the robots into Gazebo
+    spawn_robots_cmds = []
+    for robot in robots:
+        spawn_robots_cmds.append(
+            Node(
+                package='gazebo_ros',
+                executable='spawn_entity.py',
+                output='screen',
+                arguments=[
+                    '-entity', TextSubstitution(text=robot['name']),
+                    '-robot_namespace', TextSubstitution(text=robot['name']),
+                    '-file', TextSubstitution(text=sdf),
+                    '-x', TextSubstitution(text=str(robot['x_pose'])),
+                    '-y', TextSubstitution(text=str(robot['y_pose'])),
+                    '-z', TextSubstitution(text=str(robot['z_pose']))]
+            ))
+
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
+    # Define commands for launching the robot state publishers
+    robot_state_pubs_cmds = []
+    for robot in robots:
+        robot_state_pubs_cmds.append(
+            Node(
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                namespace=TextSubstitution(text=robot['name']),
+                output='screen',
+                parameters=[{'use_sim_time': True, 'robot_description': robot_description}],
+                remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')]))
+
+    # Define commands for launching the navigation instances
+    nav_instances_cmds = []
+    for robot in robots:
+        params_file = eval(f"{robot['name']}_params_file")
+
+        group = GroupAction([
+            # Instances use the robot's name for namespace
+            PushRosNamespace(robot['name']),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+                launch_arguments={
+                                  'namespace': robot['name'],
+                                  'map': map_yaml_file,
+                                  'use_sim_time': 'True',
+                                  'params_file': params_file,
+                                  'bt_xml_file': bt_xml_file,
+                                  'autostart': 'True',
+                                  'use_remappings': 'True'}.items())
+        ])
+        nav_instances_cmds.append(group)
+
+    ld = LaunchDescription()
+    ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),)
+    ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),)
+    ld.add_action(start_gazebo_cmd)
+    for spawn_robot in spawn_robots_cmds:
+        ld.add_action(spawn_robot)
+    for state_pub in robot_state_pubs_cmds:
+        ld.add_action(state_pub)
+    for nav_instance in nav_instances_cmds:
+        ld.add_action(nav_instance)
+
+    return ld
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    # TODO(orduno) remove duplicated definition of robots on `generate_launch_description`
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
+             '-rs', 'robot1', '0.0', '0.5', '1.0', '0.5',
+             '-rs', 'robot2', '0.0', '-0.5', '1.0', '-0.5',
+             '-e', 'True'],
+        name='tester_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/system/test_system_launch.py

@@ -0,0 +1,121 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+# Copyright (c) 2020 Florian Gramss
+#
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_context import LaunchContext
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml')
+
+    # Replace the default parameter values for testing special features
+    # without having multiple params_files inside the nav2 stack
+    context = LaunchContext()
+    param_substitutions = {}
+
+    if (os.getenv('ASTAR') == 'True'):
+        param_substitutions.update({'use_astar': 'True'})
+
+    param_substitutions.update(
+        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')})
+    param_substitutions.update(
+        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')})
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    new_yaml = configured_params.perform(context)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={'namespace': '',
+                              'use_namespace': 'False',
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': new_yaml,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
+             '-r', '-2.0', '-0.5', '0.0', '2.0',
+             '-e', 'True'],
+        name='tester_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/system/test_wrong_init_pose_launch.py

@@ -0,0 +1,122 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+# Copyright (c) 2020 Florian Gramss
+#
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_context import LaunchContext
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml')
+
+    # Replace the default parameter values for testing special features
+    # without having multiple params_files inside the nav2 stack
+    context = LaunchContext()
+    param_substitutions = {}
+
+    if (os.getenv('ASTAR') == 'True'):
+        param_substitutions.update({'use_astar': 'True'})
+
+    param_substitutions.update(
+        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')})
+    param_substitutions.update(
+        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')})
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    new_yaml = configured_params.perform(context)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={'namespace': '',
+                              'use_namespace': 'False',
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': new_yaml,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True',
+                              'use_composition': 'False'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
+             '-r', '-200000.0', '-200000.0', '0.0', '2.0',
+             '-e', 'False'],
+        name='tester_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/system_failure/CMakeLists.txt

@@ -0,0 +1,12 @@
+ament_add_test(test_failure_navigator
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_failure_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  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
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+)

navigation2/nav2_system_tests/src/system_failure/README.md

@@ -0,0 +1,3 @@
+# Nav2 System Tests - Failure
+
+High level system failures tests

navigation2/nav2_system_tests/src/system_failure/test_system_failure_launch.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+# Copyright (c) 2020 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_context import LaunchContext
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    param_substitutions = {
+        'planner_server.ros__parameters.GridBased.use_astar': 'False'}
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    context = LaunchContext()
+    new_yaml = configured_params.perform(context)
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={'namespace': '',
+                              'use_namespace': 'False',
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': new_yaml,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'),
+             '-r', '-2.0', '-0.5', '100.0', '100.0'],
+        name='tester_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/system_failure/tester_node.py

@@ -0,0 +1,307 @@
+#! /usr/bin/env python3
+# Copyright 2018 Intel Corporation.
+# Copyright 2020 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.
+
+import argparse
+import math
+import sys
+import time
+from typing import Optional
+
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from lifecycle_msgs.srv import GetState
+from nav2_msgs.action import NavigateToPose
+from nav2_msgs.srv import ManageLifecycleNodes
+
+import rclpy
+
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+
+
+class NavTester(Node):
+
+    def __init__(
+        self,
+        initial_pose: Pose,
+        goal_pose: Pose,
+        namespace: str = ''
+    ):
+        super().__init__(node_name='nav2_tester', namespace=namespace)
+        self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
+                                                      'initialpose', 10)
+        self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
+
+        pose_qos = QoSProfile(
+          durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+          reliability=QoSReliabilityPolicy.RELIABLE,
+          history=QoSHistoryPolicy.KEEP_LAST,
+          depth=1)
+
+        self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
+                                                       'amcl_pose', self.poseCallback, pose_qos)
+        self.initial_pose_received = False
+        self.initial_pose = initial_pose
+        self.goal_pose = goal_pose
+        self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
+
+    def info_msg(self, msg: str):
+        self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
+
+    def warn_msg(self, msg: str):
+        self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
+
+    def error_msg(self, msg: str):
+        self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
+
+    def setInitialPose(self):
+        msg = PoseWithCovarianceStamped()
+        msg.pose.pose = self.initial_pose
+        msg.header.frame_id = 'map'
+        self.info_msg('Publishing Initial Pose')
+        self.initial_pose_pub.publish(msg)
+        self.currentPose = self.initial_pose
+
+    def getStampedPoseMsg(self, pose: Pose):
+        msg = PoseStamped()
+        msg.header.frame_id = 'map'
+        msg.pose = pose
+        return msg
+
+    def publishGoalPose(self, goal_pose: Optional[Pose] = None):
+        self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
+        self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose))
+
+    def runNavigateAction(self, goal_pose: Optional[Pose] = None):
+        # Sends a `NavToPose` action request and waits for completion
+        self.info_msg("Waiting for 'NavigateToPose' action server")
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'NavigateToPose' action server not available, waiting...")
+
+        self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
+        goal_msg = NavigateToPose.Goal()
+        goal_msg.pose = self.getStampedPoseMsg(self.goal_pose)
+
+        self.info_msg('Sending goal request...')
+        send_goal_future = self.action_client.send_goal_async(goal_msg)
+
+        rclpy.spin_until_future_complete(self, send_goal_future)
+        goal_handle = send_goal_future.result()
+
+        if not goal_handle.accepted:
+            self.error_msg('Goal rejected')
+            return False
+
+        self.info_msg('Goal accepted')
+        get_result_future = goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'NavigateToPose' action to complete")
+        rclpy.spin_until_future_complete(self, get_result_future)
+        status = get_result_future.result().status
+        if status != GoalStatus.STATUS_ABORTED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+
+        self.info_msg('Goal failed, as expected!')
+        return True
+
+    def poseCallback(self, msg):
+        self.info_msg('Received amcl_pose')
+        self.current_pose = msg.pose.pose
+        self.initial_pose_received = True
+
+    def reachesGoal(self, timeout, distance):
+        goalReached = False
+        start_time = time.time()
+
+        while not goalReached:
+            rclpy.spin_once(self, timeout_sec=1)
+            if self.distanceFromGoal() < distance:
+                goalReached = True
+                self.info_msg('*** GOAL REACHED ***')
+                return True
+            elif timeout is not None:
+                if (time.time() - start_time) > timeout:
+                    self.error_msg('Robot timed out reaching its goal!')
+                    return False
+
+    def distanceFromGoal(self):
+        d_x = self.current_pose.position.x - self.goal_pose.position.x
+        d_y = self.current_pose.position.y - self.goal_pose.position.y
+        distance = math.sqrt(d_x * d_x + d_y * d_y)
+        self.info_msg(f'Distance from goal is: {distance}')
+        return distance
+
+    def wait_for_node_active(self, node_name: str):
+        # Waits for the node within the tester namespace to become active
+        self.info_msg(f'Waiting for {node_name} to become active')
+        node_service = f'{node_name}/get_state'
+        state_client = self.create_client(GetState, node_service)
+        while not state_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{node_service} service not available, waiting...')
+        req = GetState.Request()  # empty request
+        state = 'UNKNOWN'
+        while (state != 'active'):
+            self.info_msg(f'Getting {node_name} state...')
+            future = state_client.call_async(req)
+            rclpy.spin_until_future_complete(self, future)
+            if future.result() is not None:
+                state = future.result().current_state.label
+                self.info_msg(f'Result of get_state: {state}')
+            else:
+                self.error_msg(f'Exception while calling service: {future.exception()!r}')
+            time.sleep(5)
+
+    def shutdown(self):
+        self.info_msg('Shutting down')
+        self.action_client.destroy()
+
+        transition_service = 'lifecycle_manager_navigation/manage_nodes'
+        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            self.info_msg('Shutting down navigation lifecycle manager...')
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+            self.info_msg('Shutting down navigation lifecycle manager complete.')
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+        transition_service = 'lifecycle_manager_localization/manage_nodes'
+        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            self.info_msg('Shutting down localization lifecycle manager...')
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+            self.info_msg('Shutting down localization lifecycle manager complete')
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+
+    def wait_for_initial_pose(self):
+        self.initial_pose_received = False
+        while not self.initial_pose_received:
+            self.info_msg('Setting initial pose')
+            self.setInitialPose()
+            self.info_msg('Waiting for amcl_pose to be received')
+            rclpy.spin_once(self, timeout_sec=1)
+
+
+def run_all_tests(robot_tester):
+    # set transforms to use_sim_time
+    result = True
+    if (result):
+        robot_tester.wait_for_node_active('amcl')
+        robot_tester.wait_for_initial_pose()
+        robot_tester.wait_for_node_active('bt_navigator')
+        result = robot_tester.runNavigateAction()
+
+    # Add more tests here if desired
+
+    if (result):
+        robot_tester.info_msg('Test PASSED')
+    else:
+        robot_tester.error_msg('Test FAILED')
+
+    return result
+
+
+def fwd_pose(x=0.0, y=0.0, z=0.01):
+    initial_pose = Pose()
+    initial_pose.position.x = x
+    initial_pose.position.y = y
+    initial_pose.position.z = z
+    initial_pose.orientation.x = 0.0
+    initial_pose.orientation.y = 0.0
+    initial_pose.orientation.z = 0.0
+    initial_pose.orientation.w = 1.0
+    return initial_pose
+
+
+def get_testers(args):
+    testers = []
+
+    if args.robot:
+        # Requested tester for one robot
+        init_x, init_y, final_x, final_y = args.robot[0]
+        tester = NavTester(
+            initial_pose=fwd_pose(float(init_x), float(init_y)),
+            goal_pose=fwd_pose(float(final_x), float(final_y)))
+        tester.info_msg(
+            'Starting tester, robot going from ' + init_x + ', ' + init_y +
+            ' to ' + final_x + ', ' + final_y + '.')
+        testers.append(tester)
+        return testers
+
+    return testers
+
+
+def main(argv=sys.argv[1:]):
+    # The robot(s) positions from the input arguments
+    parser = argparse.ArgumentParser(description='System-level navigation tester node')
+    group = parser.add_mutually_exclusive_group(required=True)
+    group.add_argument('-r', '--robot', action='append', nargs=4,
+                       metavar=('init_x', 'init_y', 'final_x', 'final_y'),
+                       help='The robot starting and final positions.')
+    group.add_argument('-rs', '--robots', action='append', nargs=5,
+                       metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'),
+                       help="The robot's namespace and starting and final positions. " +
+                            'Repeating the argument for multiple robots is supported.')
+
+    args, unknown = parser.parse_known_args()
+
+    rclpy.init()
+
+    # Create testers for each robot
+    testers = get_testers(args)
+
+    # wait a few seconds to make sure entire stacks are up
+    time.sleep(10)
+
+    for tester in testers:
+        passed = run_all_tests(tester)
+        if not passed:
+            break
+
+    for tester in testers:
+        # stop and shutdown the nav stack to exit cleanly
+        tester.shutdown()
+
+    testers[0].info_msg('Done Shutting Down.')
+
+    if not passed:
+        testers[0].info_msg('Exiting failed')
+        exit(1)
+    else:
+        testers[0].info_msg('Exiting passed')
+        exit(0)
+
+
+if __name__ == '__main__':
+    main()

navigation2/nav2_system_tests/src/updown/CMakeLists.txt

@@ -0,0 +1,11 @@
+add_executable(test_updown
+  test_updown.cpp
+)
+
+ament_target_dependencies(test_updown
+  ${dependencies}
+)
+
+install(TARGETS test_updown RUNTIME DESTINATION lib/${PROJECT_NAME})
+install(FILES test_updown_launch.py DESTINATION share/${PROJECT_NAME})
+

navigation2/nav2_system_tests/src/updown/README.md

@@ -0,0 +1,22 @@
+# Nav2 Updown Test
+
+This is a 'top level' system test which tests the lifecycle bringup and shutdown of the system. 
+
+## To run the test
+```
+ros2 launch nav2_system_tests test_updown_launch.py
+```
+
+If the test passes, you should see this comment in the output:
+```
+[test_updown-13] [INFO] [test_updown]: ****************************************************  TEST PASSED!
+```
+
+To run the test in a loop 1000x, run the `test_updown_reliablity` script and log the output:
+```
+./test_updown_reliablity |& tee /tmp/updown.log
+```
+When the test is completed, pipe the log to the `updownresults.py` script to get a summary of the results:
+```
+./updownresults.py < /tmp/updown.log`
+```

navigation2/nav2_system_tests/src/updown/start_nav2

@@ -0,0 +1,2 @@
+#!/bin/bash
+ros2 launch ./test_updown_launch.py

navigation2/nav2_system_tests/src/updown/test_updown.cpp

@@ -0,0 +1,77 @@
+// 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 <random>
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
+#include "rcutils/cmdline_parser.h"
+
+using namespace std::chrono_literals;
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  RCLCPP_INFO(rclcpp::get_logger("test_updown"), "Initializing test");
+  auto node = std::make_shared<rclcpp::Node>("lifecycle_manager_service_client");
+  nav2_lifecycle_manager::LifecycleManagerClient client_nav("lifecycle_manager_navigation", node);
+  nav2_lifecycle_manager::LifecycleManagerClient client_loc("lifecycle_manager_localization", node);
+  bool test_passed = true;
+
+  // Wait for a few seconds to let all of the nodes come up
+  std::this_thread::sleep_for(5s);
+
+  // Start the nav2 system, bringing it to the ACTIVE state
+  client_nav.startup();
+  client_loc.startup();
+
+  // Wait for a couple secs to make sure the nodes have processed all discovery
+  // info before starting
+  RCLCPP_INFO(rclcpp::get_logger("test_updown"), "Waiting for nodes to be active");
+  std::this_thread::sleep_for(2s);
+
+  // The system should now be active
+  int retries = 0;
+  while ((client_nav.is_active() != nav2_lifecycle_manager::SystemStatus::ACTIVE) &&
+    (client_loc.is_active() != nav2_lifecycle_manager::SystemStatus::ACTIVE) &&
+    (retries < 10))
+  {
+    std::this_thread::sleep_for(2s);
+    retries++;
+  }
+  if (retries == 10) {
+    // the system isn't active
+    RCLCPP_ERROR(rclcpp::get_logger("test_updown"), "System startup failed");
+    test_passed = false;
+  }
+
+  // Shut down the nav2 system, bringing it to the FINALIZED state
+  client_nav.shutdown();
+  client_loc.shutdown();
+
+  if (test_passed) {
+    RCLCPP_INFO(
+      rclcpp::get_logger("test_updown"),
+      "****************************************************  TEST PASSED!");
+  } else {
+    RCLCPP_INFO(
+      rclcpp::get_logger("test_updown"),
+      "****************************************************  TEST FAILED!");
+  }
+  rclcpp::shutdown();
+  return 0;
+}

navigation2/nav2_system_tests/src/updown/test_updown_launch.py

@@ -0,0 +1,91 @@
+# Copyright (c) 2019 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.
+
+import os
+
+from ament_index_python.packages import get_package_prefix
+from ament_index_python.packages import get_package_share_directory
+
+import launch.actions
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    # Configuration parameters for the launch
+    launch_dir = os.path.join(
+        get_package_share_directory('nav2_bringup'), 'launch')
+
+    map_yaml_file = os.path.join(
+        get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml')
+
+    # Specify the actions
+    start_tf_cmd_1 = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        output='screen',
+        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'])
+
+    start_tf_cmd_2 = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        output='screen',
+        arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint'])
+
+    start_tf_cmd_3 = Node(
+       package='tf2_ros',
+       executable='static_transform_publisher',
+       output='screen',
+       arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'])
+
+    start_tf_cmd_4 = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        output='screen',
+        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'])
+
+    nav2_bringup = launch.actions.IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir, 'bringup_launch.py')),
+        launch_arguments={'map': map_yaml_file,
+                          'use_sim_time': 'True',
+                          'autostart': 'False'}.items())
+
+    start_test = launch.actions.ExecuteProcess(
+        cmd=[
+            os.path.join(
+                get_package_prefix('nav2_system_tests'),
+                'lib/nav2_system_tests/test_updown')],
+        cwd=[launch_dir], output='screen')
+
+    test_exit_event_handler = launch.actions.RegisterEventHandler(
+        event_handler=launch.event_handlers.OnProcessExit(
+            target_action=start_test,
+            on_exit=launch.actions.EmitEvent(event=launch.events.Shutdown(reason='Done!'))))
+
+    # Compose the launch description
+
+    ld = launch.LaunchDescription()
+
+    ld.add_action(start_tf_cmd_1)
+    ld.add_action(start_tf_cmd_2)
+    ld.add_action(start_tf_cmd_3)
+    ld.add_action(start_tf_cmd_4)
+
+    ld.add_action(nav2_bringup)
+    ld.add_action(start_test)
+    ld.add_action(test_exit_event_handler)
+
+    return ld

navigation2/nav2_system_tests/src/updown/test_updown_reliability

@@ -0,0 +1,22 @@
+#!/bin/bash
+for i in `seq 1 1000`;
+do
+    echo "======= START OF RUN: $i ========="
+
+    # Start with a new ros2 daemon 
+    ros2 daemon stop
+
+    # There shouldn't be any nodes in the list
+    ros2 node list
+    echo "----------------------------------"
+
+    # Bound the time of the bringup/shutdown in case it hangs
+    timeout 100 ./start_nav2
+
+    # Make sure there aren't any stray gazebo or ros2 processes hanging around
+    # that were not properly killed by the launch script
+    #kill -9 $(pgrep gzserver) &> /dev/null
+    kill -9 $(pgrep ros2) &> /dev/null
+
+    echo "======== END OF RUN: $i =========="
+done

navigation2/nav2_system_tests/src/updown/updownresults.py

@@ -0,0 +1,72 @@
+#!/usr/bin/python3
+# Copyright (c) 2019 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.
+
+
+# To use this script, run the `test_updown_reliablity` script and log the output
+# > ./test_updown_reliablity |& tee /tmp/updown.log
+# When the test is completed, pipe the log to this script to get a summary of the
+# results
+# > ./updownresults.py < /tmp/updown.log
+#
+# This reports the number of successful tests, but also the number of times the
+# tests were able to make it to the active state as well as the shutdown state.
+# It can frequently occur that the system makes all the lifecycle state transitions
+# but has an error during the final process termination.
+
+import sys
+
+
+def main():
+    log = sys.stdin
+    test_count = 0
+    fail_count = 0
+    successful_bringup_count = 0
+    successful_shutdown_count = 0
+    for line in log.readlines():
+        if line.startswith('======= START OF RUN:'):
+            test_successful = True
+            shutdown_successful = False
+            bringup_successful = False
+
+        if line.startswith('======== END OF RUN:'):
+            test_count += 1
+            conclusion = ''
+            if bringup_successful:
+                successful_bringup_count += 1
+                conclusion = ' but bringup was successful'
+            if shutdown_successful:
+                successful_shutdown_count += 1
+                conclusion = ' but shutdown was successful'
+            if not test_successful:
+                fail_count += 1
+                print('Failure in test ', test_count, conclusion)
+
+        if '[ERROR]' in line:
+            test_successful = False
+
+        if 'The system is active' in line:
+            bringup_successful = True
+
+        if 'The system has been sucessfully shut down' in line:
+            shutdown_successful = True
+
+    print('Number of tests: ', test_count)
+    print('Number of successes: ', test_count-fail_count)
+    print('Number of successful bringups', successful_bringup_count)
+    print('Number of successful shutdowns', successful_shutdown_count)
+
+
+if __name__ == '__main__':
+    main()

navigation2/nav2_system_tests/src/waypoint_follower/CMakeLists.txt

@@ -0,0 +1,12 @@
+ament_add_test(test_waypoint_follower
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_case_py.launch"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  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
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
+)

navigation2/nav2_system_tests/src/waypoint_follower/README.md

@@ -0,0 +1,3 @@
+# Waypoint Follower Test
+
+This is a simple test for the waypoint follower. It creates an instance of the stack, calls with waypoint follower, and checks for successful navigation with 4 predetermined waypoints in the turtlebot map.

navigation2/nav2_system_tests/src/waypoint_follower/test_case_py.launch

@@ -0,0 +1,104 @@
+#! /usr/bin/env python3
+# 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_context import LaunchContext
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    map_yaml_file = os.getenv('TEST_MAP')
+    world = os.getenv('TEST_WORLD')
+
+    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
+                                    'behavior_trees',
+                                    os.getenv('BT_NAVIGATOR_XML'))
+
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
+
+    # Replace the `use_astar` setting on the params file
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites='',
+        convert_types=True)
+
+    context = LaunchContext()
+    new_yaml = configured_params.perform(context)
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+
+        # Launch gazebo server for simulation
+        ExecuteProcess(
+            cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
+                 '--minimal_comms', world],
+            output='screen'),
+
+        # TODO(orduno) Launch the robot state publisher instead
+        #              using a local copy of TB3 urdf file
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']),
+
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(bringup_dir, 'launch', 'bringup_launch.py')),
+            launch_arguments={
+                              'map': map_yaml_file,
+                              'use_sim_time': 'True',
+                              'params_file': new_yaml,
+                              'bt_xml_file': bt_navigator_xml,
+                              'autostart': 'True'}.items()),
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester.py')],
+        name='tester_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_system_tests/src/waypoint_follower/tester.py

@@ -0,0 +1,230 @@
+#! /usr/bin/env python3
+# Copyright 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.
+
+import sys
+import time
+
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
+from nav2_msgs.action import FollowWaypoints
+from nav2_msgs.srv import ManageLifecycleNodes
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+
+
+class WaypointFollowerTest(Node):
+
+    def __init__(self):
+        super().__init__(node_name='nav2_waypoint_tester', namespace='')
+        self.waypoints = None
+        self.action_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')
+        self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
+                                                      'initialpose', 10)
+        self.initial_pose_received = False
+        self.goal_handle = None
+
+        pose_qos = QoSProfile(
+          durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+          reliability=QoSReliabilityPolicy.RELIABLE,
+          history=QoSHistoryPolicy.KEEP_LAST,
+          depth=1)
+
+        self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
+                                                       'amcl_pose', self.poseCallback, pose_qos)
+
+    def setInitialPose(self, pose):
+        self.init_pose = PoseWithCovarianceStamped()
+        self.init_pose.pose.pose.position.x = pose[0]
+        self.init_pose.pose.pose.position.y = pose[1]
+        self.init_pose.header.frame_id = 'map'
+        self.publishInitialPose()
+        time.sleep(5)
+
+    def poseCallback(self, msg):
+        self.info_msg('Received amcl_pose')
+        self.initial_pose_received = True
+
+    def setWaypoints(self, waypoints):
+        self.waypoints = []
+        for wp in waypoints:
+            msg = PoseStamped()
+            msg.header.frame_id = 'map'
+            msg.pose.position.x = wp[0]
+            msg.pose.position.y = wp[1]
+            msg.pose.orientation.w = 1.0
+            self.waypoints.append(msg)
+
+    def run(self, block):
+        if not self.waypoints:
+            rclpy.error_msg('Did not set valid waypoints before running test!')
+            return False
+
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'follow_waypoints' action server not available, waiting...")
+
+        action_request = FollowWaypoints.Goal()
+        action_request.poses = self.waypoints
+
+        self.info_msg('Sending goal request...')
+        send_goal_future = self.action_client.send_goal_async(action_request)
+        try:
+            rclpy.spin_until_future_complete(self, send_goal_future)
+            self.goal_handle = send_goal_future.result()
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+
+        if not self.goal_handle.accepted:
+            self.error_msg('Goal rejected')
+            return False
+
+        self.info_msg('Goal accepted')
+        if not block:
+            return True
+
+        get_result_future = self.goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'follow_waypoints' action to complete")
+        try:
+            rclpy.spin_until_future_complete(self, get_result_future)
+            status = get_result_future.result().status
+            result = get_result_future.result().result
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'Service call failed {e!r}')
+
+        if status != GoalStatus.STATUS_SUCCEEDED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+        if len(result.missed_waypoints) > 0:
+            self.info_msg('Goal failed to process all waypoints,'
+                          ' missed {0} wps.'.format(len(result.missed_waypoints)))
+            return False
+
+        self.info_msg('Goal succeeded!')
+        return True
+
+    def publishInitialPose(self):
+        self.initial_pose_pub.publish(self.init_pose)
+
+    def shutdown(self):
+        self.info_msg('Shutting down')
+
+        self.action_client.destroy()
+        self.info_msg('Destroyed follow_waypoints action client')
+
+        transition_service = 'lifecycle_manager_navigation/manage_nodes'
+        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'{transition_service} service call failed {e!r}')
+
+        self.info_msg(f'{transition_service} finished')
+
+        transition_service = 'lifecycle_manager_localization/manage_nodes'
+        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        while not mgr_client.wait_for_service(timeout_sec=1.0):
+            self.info_msg(f'{transition_service} service not available, waiting...')
+
+        req = ManageLifecycleNodes.Request()
+        req.command = ManageLifecycleNodes.Request().SHUTDOWN
+        future = mgr_client.call_async(req)
+        try:
+            rclpy.spin_until_future_complete(self, future)
+            future.result()
+        except Exception as e:  # noqa: B902
+            self.error_msg(f'{transition_service} service call failed {e!r}')
+
+        self.info_msg(f'{transition_service} finished')
+
+    def cancel_goal(self):
+        cancel_future = self.goal_handle.cancel_goal_async()
+        rclpy.spin_until_future_complete(self, cancel_future)
+
+    def info_msg(self, msg: str):
+        self.get_logger().info(msg)
+
+    def warn_msg(self, msg: str):
+        self.get_logger().warn(msg)
+
+    def error_msg(self, msg: str):
+        self.get_logger().error(msg)
+
+
+def main(argv=sys.argv[1:]):
+    rclpy.init()
+
+    # wait a few seconds to make sure entire stacks are up
+    time.sleep(10)
+
+    wps = [[-0.52, -0.54], [0.58, -0.55], [0.58, 0.52]]
+    starting_pose = [-2.0, -0.5]
+
+    test = WaypointFollowerTest()
+    test.setWaypoints(wps)
+
+    retry_count = 0
+    retries = 2
+    while not test.initial_pose_received and retry_count <= retries:
+        retry_count += 1
+        test.info_msg('Setting initial pose')
+        test.setInitialPose(starting_pose)
+        test.info_msg('Waiting for amcl_pose to be received')
+        rclpy.spin_once(test, timeout_sec=1.0)  # wait for poseCallback
+
+    result = test.run(True)
+    assert result
+
+    # preempt with new point
+    test.setWaypoints([starting_pose])
+    result = test.run(False)
+    time.sleep(2)
+    test.setWaypoints([wps[1]])
+    result = test.run(False)
+
+    # cancel
+    time.sleep(2)
+    test.cancel_goal()
+
+    # a failure case
+    time.sleep(2)
+    test.setWaypoints([[100.0, 100.0]])
+    result = test.run(True)
+    assert not result
+    result = not result
+
+    test.shutdown()
+    test.info_msg('Done Shutting Down.')
+
+    if not result:
+        test.info_msg('Exiting failed')
+        exit(1)
+    else:
+        test.info_msg('Exiting passed')
+        exit(0)
+
+
+if __name__ == '__main__':
+    main()