diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index 9e67c0a48b..127986f464 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -120,8 +120,8 @@ if(BUILD_TESTING)
   # add_subdirectory(src/gps_navigation)
   add_subdirectory(src/behaviors/wait)
   add_subdirectory(src/behaviors/spin)
-  # add_subdirectory(src/behaviors/backup)
-  # add_subdirectory(src/behaviors/drive_on_heading)
+  add_subdirectory(src/behaviors/backup)
+  add_subdirectory(src/behaviors/drive_on_heading)
   add_subdirectory(src/behaviors/assisted_teleop)
   add_subdirectory(src/costmap_filters)
   add_subdirectory(src/error_codes)
diff --git a/nav2_system_tests/src/behaviors/backup/CMakeLists.txt b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt
index 05afe3c50d..b5ab0a60b1 100644
--- a/nav2_system_tests/src/behaviors/backup/CMakeLists.txt
+++ b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt
@@ -1,23 +1,8 @@
-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
+ament_add_test(test_backup_behavior
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_behavior_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_behavior.launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
-  TIMEOUT 180
+  TIMEOUT 60
   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
+    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
 )
diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp
deleted file mode 100644
index 974c2d60b2..0000000000
--- a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp
+++ /dev/null
@@ -1,223 +0,0 @@
-// 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
diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp
deleted file mode 100644
index bf42c9881a..0000000000
--- a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp
+++ /dev/null
@@ -1,90 +0,0 @@
-// 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_
diff --git a/nav2_system_tests/src/behaviors/backup/backup_tester.py b/nav2_system_tests/src/behaviors/backup/backup_tester.py
new file mode 100755
index 0000000000..ad6f724bf8
--- /dev/null
+++ b/nav2_system_tests/src/behaviors/backup/backup_tester.py
@@ -0,0 +1,332 @@
+#! /usr/bin/env python3
+# Copyright 2024 Open Navigation LLC
+#
+# 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 Point32, PolygonStamped
+from nav2_msgs.action import BackUp
+from nav2_msgs.msg import Costmap
+from nav2_msgs.srv import ManageLifecycleNodes
+
+import rclpy
+
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+
+
+class BackupTest(Node):
+
+    def __init__(self):
+        super().__init__(node_name='backup_tester', namespace='')
+        self.costmap_qos = QoSProfile(
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+        self.action_client = ActionClient(self, BackUp, 'backup')
+        self.costmap_pub = self.create_publisher(
+            Costmap, 'local_costmap/costmap_raw', self.costmap_qos)
+        self.footprint_pub = self.create_publisher(
+            PolygonStamped, 'local_costmap/published_footprint', 10)
+        self.goal_handle = None
+        self.action_result = None
+
+    def sendCommand(self, command):
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'Backup' action to complete")
+        try:
+            rclpy.spin_until_future_complete(self, self.result_future)
+            status = self.result_future.result().status
+            result = self.result_future.result().result
+            self.action_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 self.action_result.error_code == 0:
+            self.info_msg('Backup was successful!')
+            return True
+        self.info_msg('Backup failed to meet target!')
+        return False
+
+    def sendAndPreemptWithFasterCommand(self, command):
+        # Send initial goal
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Now preempt it
+        time.sleep(0.5)
+        self.info_msg('Sending preemption request...')
+        command.speed = 0.2
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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('Preemption rejected')
+            return False
+
+        self.info_msg('Preemption accepted')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Wait for new goal to complete
+        self.info_msg("Waiting for 'backup' action Preemption to complete")
+        try:
+            rclpy.spin_until_future_complete(self, self.result_future)
+            status = self.result_future.result().status
+            result = self.result_future.result().result
+            self.action_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 self.action_result.error_code == 0:
+            self.info_msg('Backup was successful!')
+            return True
+        self.info_msg('Backup failed to meet target!')
+        return False
+
+    def sendAndCancelCommand(self, command):
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Now cancel it
+        time.sleep(0.5)
+        cancel_future = self.goal_handle.cancel_goal_async()
+        rclpy.spin_until_future_complete(self, cancel_future)
+        rclpy.spin_until_future_complete(self, self.result_future)
+        status = self.result_future.result().status
+        if status != GoalStatus.STATUS_CANCELED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+        else:
+            self.info_msg('Goal was canceled successfully')
+            return True
+
+    def sendFreeCostmap(self):
+        costmap_msg = Costmap()
+        costmap_msg.header.frame_id = 'odom'
+        costmap_msg.header.stamp = self.get_clock().now().to_msg()
+        costmap_msg.metadata.resolution = 0.05
+        costmap_msg.metadata.size_x = 100
+        costmap_msg.metadata.size_y = 100
+        costmap_msg.metadata.origin.position.x = -2.5
+        costmap_msg.metadata.origin.position.y = -2.5
+        costmap_msg.data = [0] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y)
+        self.costmap_pub.publish(costmap_msg)
+
+        footprint_msg = PolygonStamped()
+        footprint_msg.header.frame_id = 'odom'
+        footprint_msg.header.stamp = self.get_clock().now().to_msg()
+        footprint_msg.polygon.points = [
+            Point32(x=0.1, y=0.1, z=0.0),
+            Point32(x=0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=0.1, z=0.0)
+        ]
+        self.footprint_pub.publish(footprint_msg)
+
+    def sendOccupiedCostmap(self):
+        costmap_msg = Costmap()
+        costmap_msg.header.frame_id = 'odom'
+        costmap_msg.header.stamp = self.get_clock().now().to_msg()
+        costmap_msg.metadata.resolution = 0.05
+        costmap_msg.metadata.size_x = 100
+        costmap_msg.metadata.size_y = 100
+        costmap_msg.metadata.origin.position.x = -2.5
+        costmap_msg.metadata.origin.position.y = -2.5
+        costmap_msg.data = [254] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y)
+        self.costmap_pub.publish(costmap_msg)
+
+        footprint_msg = PolygonStamped()
+        footprint_msg.header.frame_id = 'odom'
+        footprint_msg.header.stamp = self.get_clock().now().to_msg()
+        footprint_msg.polygon.points = [
+            Point32(x=0.1, y=0.1, z=0.0),
+            Point32(x=0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=0.1, z=0.0)
+        ]
+        self.footprint_pub.publish(footprint_msg)
+
+    def run(self):
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'Backup' action server not available, waiting...")
+
+        # Test A: Send without valid costmap
+        action_request = BackUp.Goal()
+        action_request.speed = 0.15
+        action_request.target.x = 0.15
+        action_request.time_allowance = Duration(seconds=5).to_msg()
+        cmd1 = self.sendCommand(action_request)
+
+        if cmd1:
+            self.error_msg('Test A failed: Passed while costmap was not available!')
+            return not cmd1
+        else:
+            self.info_msg('Test A passed')
+
+        # Test B: Send with valid costmap and Backup a couple of times
+        self.sendFreeCostmap()
+        time.sleep(1)
+        cmd1 = self.sendCommand(action_request)
+        action_request.target.x = 0.1
+        cmd2 = self.sendCommand(action_request)
+
+        if not cmd1 or not cmd2:
+            self.error_msg('Test B failed: Failed to Backup with valid costmap!')
+            return not cmd1 or not cmd2
+
+        action_request.target.x = 0.5
+        cmd_preempt = self.sendAndPreemptWithFasterCommand(action_request)
+        if not cmd_preempt:
+            self.error_msg('Test B failed: Failed to preempt and invert command!')
+            return not cmd_preempt
+
+        cmd_cancel = self.sendAndCancelCommand(action_request)
+        if not cmd_cancel:
+            self.error_msg('Test B failed: Failed to cancel command!')
+            return not cmd_cancel
+        else:
+            self.info_msg('Test B passed')
+
+        # Test C: Send with impossible command in time allowance & target
+        action_request.time_allowance = Duration(seconds=0.1).to_msg()
+        cmd3 = self.sendCommand(action_request)
+        if cmd3:
+            self.error_msg('Test C failed: Passed while impoossible timing requested!')
+            return not cmd3
+
+        action_request.target.y = 0.5
+        cmd_invalid_target = self.sendCommand(action_request)
+        if cmd_invalid_target:
+            self.error_msg('Test C failed: Passed while impoossible target requested!')
+            return not cmd_invalid_target
+        else:
+            action_request.target.y = 0.0
+            self.info_msg('Test C passed')
+
+        # Test D: Send with lethal costmap and Backup
+        action_request.time_allowance = Duration(seconds=5).to_msg()
+        self.sendOccupiedCostmap()
+        time.sleep(1)
+        cmd4 = self.sendCommand(action_request)
+        if cmd4:
+            self.error_msg('Test D failed: Passed while costmap was not lethal!')
+            return not cmd4
+        else:
+            self.info_msg('Test D passed')
+        return True
+
+    def shutdown(self):
+        self.info_msg('Shutting down')
+
+        self.action_client.destroy()
+        self.info_msg('Destroyed backup 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')
+
+    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()
+    time.sleep(10)
+    test = BackupTest()
+    result = test.run()
+    test.shutdown()
+
+    if not result:
+        test.info_msg('Exiting failed')
+        exit(1)
+    else:
+        test.info_msg('Exiting passed')
+        exit(0)
+
+
+if __name__ == '__main__':
+    main()
diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py
similarity index 51%
rename from nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py
rename to nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py
index d18b29d5b9..d48c6ca0e7 100755
--- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py
+++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py
@@ -14,13 +14,20 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 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.actions import (
+    AppendEnvironmentVariable,
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
@@ -28,101 +35,69 @@
 
 
 def generate_launch_description():
-
     bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
-
+    params_file = LaunchConfiguration('params_file')
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro')
     urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
     with open(urdf, 'r') as infp:
         robot_description = infp.read()
 
-    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,
-    }
-
+    param_substitutions = {'use_sim_time': 'True'}
     configured_params = RewrittenYaml(
         source_file=params_file,
-        root_key=namespace,
+        root_key='',
         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',
+            # Simulation for odometry
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
             ),
-            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',
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                str(Path(os.path.join(sim_dir)).parent.resolve())
             ),
-            DeclareLaunchArgument(
-                'map_subscribe_transient_local',
-                default_value='false',
-                description='Whether to set the map subscriber QoS to transient local',
+            ExecuteProcess(
+                cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro],
+                output='screen',
             ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
+                ),
+                launch_arguments={
+                    'use_sim_time': 'True',
+                    'robot_sdf': robot_sdf,
+                    'x_pose': '-2.0',
+                    'y_pose': '-0.5',
+                    'z_pose': '0.01',
+                    'roll': '0.0',
+                    'pitch': '0.0',
+                    'yaw': '0.0',
+                }.items(),
+            ),
+            # No need for localization
             Node(
                 package='tf2_ros',
                 executable='static_transform_publisher',
                 output='screen',
                 arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
+                parameters=[{'use_sim_time': True}],
             ),
+            # Need transforms
             Node(
                 package='robot_state_publisher',
                 executable='robot_state_publisher',
@@ -132,13 +107,13 @@ def generate_launch_description():
                     {'use_sim_time': True, 'robot_description': robot_description}
                 ],
             ),
+            # Server under test
             Node(
                 package='nav2_behaviors',
                 executable='behavior_server',
                 name='behavior_server',
                 output='screen',
                 parameters=[configured_params],
-                remappings=remappings,
             ),
             Node(
                 package='nav2_lifecycle_manager',
@@ -146,9 +121,9 @@ def generate_launch_description():
                 name='lifecycle_manager_navigation',
                 output='screen',
                 parameters=[
-                    {'use_sim_time': use_sim_time},
-                    {'autostart': autostart},
-                    {'node_names': lifecycle_nodes},
+                    {'use_sim_time': True},
+                    {'autostart': True},
+                    {'node_names': ['behavior_server']},
                 ],
             ),
         ]
@@ -158,17 +133,19 @@ def generate_launch_description():
 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'
+        cmd=[os.path.join(
+            os.getenv('TEST_DIR'), 'backup_tester.py'), '--ros-args', '-p', 'use_sim_time:=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)
+    return_code = lts.run(ls)
+    return return_code
 
 
 if __name__ == '__main__':
diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py
deleted file mode 100755
index bd4c5432fe..0000000000
--- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py
+++ /dev/null
@@ -1,116 +0,0 @@
-#! /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,
-                    'use_composition': 'False',
-                    '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())
diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp
deleted file mode 100644
index e629dca0bb..0000000000
--- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-// 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;
-}
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt b/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt
index fe17417e16..4f42fe3aca 100644
--- a/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt
+++ b/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt
@@ -1,23 +1,8 @@
-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
+ament_add_test(test_drive_on_heading_behavior
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_drive_on_heading_behavior_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_drive_on_heading_behavior.launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
-  TIMEOUT 180
+  TIMEOUT 60
   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
+    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
 )
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp
deleted file mode 100644
index 967c6beb04..0000000000
--- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp
+++ /dev/null
@@ -1,224 +0,0 @@
-// 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
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp
deleted file mode 100644
index 920164f162..0000000000
--- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp
+++ /dev/null
@@ -1,90 +0,0 @@
-// 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_
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py
new file mode 100755
index 0000000000..2f057620ba
--- /dev/null
+++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py
@@ -0,0 +1,340 @@
+#! /usr/bin/env python3
+# Copyright 2024 Open Navigation LLC
+#
+# 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 Point32, PolygonStamped
+from nav2_msgs.action import DriveOnHeading
+from nav2_msgs.msg import Costmap
+from nav2_msgs.srv import ManageLifecycleNodes
+
+import rclpy
+
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+
+
+class DriveTest(Node):
+
+    def __init__(self):
+        super().__init__(node_name='drive_tester', namespace='')
+        self.costmap_qos = QoSProfile(
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+        self.action_client = ActionClient(self, DriveOnHeading, 'drive_on_heading')
+        self.costmap_pub = self.create_publisher(
+            Costmap, 'local_costmap/costmap_raw', self.costmap_qos)
+        self.footprint_pub = self.create_publisher(
+            PolygonStamped, 'local_costmap/published_footprint', 10)
+        self.goal_handle = None
+        self.action_result = None
+
+    def sendCommand(self, command):
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'DriveOnHeading' action to complete")
+        try:
+            rclpy.spin_until_future_complete(self, self.result_future)
+            status = self.result_future.result().status
+            result = self.result_future.result().result
+            self.action_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 self.action_result.error_code == 0:
+            self.info_msg('DriveOnHeading was successful!')
+            return True
+        self.info_msg('DriveOnHeading failed to meet target!')
+        return False
+
+    def sendAndPreemptWithFasterCommand(self, command):
+        # Send initial goal
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Now preempt it
+        time.sleep(0.5)
+        self.info_msg('Sending preemption request...')
+        command.speed = 0.2
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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('Preemption rejected')
+            return False
+
+        self.info_msg('Preemption accepted')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Wait for new goal to complete
+        self.info_msg("Waiting for 'DriveOnHeading' action Preemption to complete")
+        try:
+            rclpy.spin_until_future_complete(self, self.result_future)
+            status = self.result_future.result().status
+            result = self.result_future.result().result
+            self.action_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 self.action_result.error_code == 0:
+            self.info_msg('DriveOnHeading was successful!')
+            return True
+        self.info_msg('DriveOnHeading failed to meet target!')
+        return False
+
+    def sendAndCancelCommand(self, command):
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Now cancel it
+        time.sleep(0.5)
+        cancel_future = self.goal_handle.cancel_goal_async()
+        rclpy.spin_until_future_complete(self, cancel_future)
+        rclpy.spin_until_future_complete(self, self.result_future)
+        status = self.result_future.result().status
+        if status != GoalStatus.STATUS_CANCELED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+        else:
+            self.info_msg('Goal was canceled successfully')
+            return True
+
+    def sendFreeCostmap(self):
+        costmap_msg = Costmap()
+        costmap_msg.header.frame_id = 'odom'
+        costmap_msg.header.stamp = self.get_clock().now().to_msg()
+        costmap_msg.metadata.resolution = 0.05
+        costmap_msg.metadata.size_x = 100
+        costmap_msg.metadata.size_y = 100
+        costmap_msg.metadata.origin.position.x = -2.5
+        costmap_msg.metadata.origin.position.y = -2.5
+        costmap_msg.data = [0] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y)
+        self.costmap_pub.publish(costmap_msg)
+
+        footprint_msg = PolygonStamped()
+        footprint_msg.header.frame_id = 'odom'
+        footprint_msg.header.stamp = self.get_clock().now().to_msg()
+        footprint_msg.polygon.points = [
+            Point32(x=0.1, y=0.1, z=0.0),
+            Point32(x=0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=0.1, z=0.0)
+        ]
+        self.footprint_pub.publish(footprint_msg)
+
+    def sendOccupiedCostmap(self):
+        costmap_msg = Costmap()
+        costmap_msg.header.frame_id = 'odom'
+        costmap_msg.header.stamp = self.get_clock().now().to_msg()
+        costmap_msg.metadata.resolution = 0.05
+        costmap_msg.metadata.size_x = 100
+        costmap_msg.metadata.size_y = 100
+        costmap_msg.metadata.origin.position.x = -2.5
+        costmap_msg.metadata.origin.position.y = -2.5
+        costmap_msg.data = [254] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y)
+        self.costmap_pub.publish(costmap_msg)
+
+        footprint_msg = PolygonStamped()
+        footprint_msg.header.frame_id = 'odom'
+        footprint_msg.header.stamp = self.get_clock().now().to_msg()
+        footprint_msg.polygon.points = [
+            Point32(x=0.1, y=0.1, z=0.0),
+            Point32(x=0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=0.1, z=0.0)
+        ]
+        self.footprint_pub.publish(footprint_msg)
+
+    def run(self):
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'DriveOnHeading' action server not available, waiting...")
+
+        # Test A: Send without valid costmap
+        action_request = DriveOnHeading.Goal()
+        action_request.speed = 0.15
+        action_request.target.x = 0.15
+        action_request.time_allowance = Duration(seconds=5).to_msg()
+        cmd1 = self.sendCommand(action_request)
+
+        if cmd1:
+            self.error_msg('Test A failed: Passed while costmap was not available!')
+            return not cmd1
+        else:
+            self.info_msg('Test A passed')
+
+        # Test B: Send with valid costmap and DriveOnHeading a couple of times
+        self.sendFreeCostmap()
+        time.sleep(1)
+        cmd1 = self.sendCommand(action_request)
+        action_request.target.x = 0.1
+        cmd2 = self.sendCommand(action_request)
+
+        if not cmd1 or not cmd2:
+            self.error_msg('Test B failed: Failed to DriveOnHeading with valid costmap!')
+            return not cmd1 or not cmd2
+
+        action_request.target.x = 0.5
+        cmd_preempt = self.sendAndPreemptWithFasterCommand(action_request)
+        if not cmd_preempt:
+            self.error_msg('Test B failed: Failed to preempt and invert command!')
+            return not cmd_preempt
+
+        cmd_cancel = self.sendAndCancelCommand(action_request)
+        if not cmd_cancel:
+            self.error_msg('Test B failed: Failed to cancel command!')
+            return not cmd_cancel
+        else:
+            self.info_msg('Test B passed')
+
+        # Test C: Send with impossible command in time allowance & target * signs
+        action_request.time_allowance = Duration(seconds=0.1).to_msg()
+        cmd3 = self.sendCommand(action_request)
+        if cmd3:
+            self.error_msg('Test C failed: Passed while impoossible timing requested!')
+            return not cmd3
+
+        action_request.target.y = 0.5
+        cmd_invalid_target = self.sendCommand(action_request)
+        if cmd_invalid_target:
+            self.error_msg('Test C failed: Passed while impoossible target requested!')
+            return not cmd_invalid_target
+        else:
+            action_request.target.y = 0.0
+
+        action_request.target.x = -0.5
+        cmd_invalid_sign = self.sendCommand(action_request)
+        if cmd_invalid_sign:
+            self.error_msg('Test C failed: Passed while impoossible target sign requested!')
+            return not cmd_invalid_sign
+        else:
+            action_request.target.x = 0.5
+            self.info_msg('Test C passed')
+
+        # Test D: Send with lethal costmap and DriveOnHeading
+        action_request.time_allowance = Duration(seconds=5).to_msg()
+        self.sendOccupiedCostmap()
+        time.sleep(1)
+        cmd4 = self.sendCommand(action_request)
+        if cmd4:
+            self.error_msg('Test D failed: Passed while costmap was not lethal!')
+            return not cmd4
+        else:
+            self.info_msg('Test D passed')
+        return True
+
+    def shutdown(self):
+        self.info_msg('Shutting down')
+
+        self.action_client.destroy()
+        self.info_msg('Destroyed DriveOnHeading 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')
+
+    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()
+    time.sleep(10)
+    test = DriveTest()
+    result = test.run()
+    test.shutdown()
+
+    if not result:
+        test.info_msg('Exiting failed')
+        exit(1)
+    else:
+        test.info_msg('Exiting passed')
+        exit(0)
+
+
+if __name__ == '__main__':
+    main()
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py
new file mode 100755
index 0000000000..ce6c381cd9
--- /dev/null
+++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py
@@ -0,0 +1,152 @@
+#! /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
+from pathlib import Path
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import (
+    AppendEnvironmentVariable,
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+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')
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
+    params_file = LaunchConfiguration('params_file')
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro')
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {'use_sim_time': 'True'}
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True,
+    )
+
+    return LaunchDescription(
+        [
+            SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+            SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+            DeclareLaunchArgument(
+                'params_file',
+                default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+                description='Full path to the ROS2 parameters file to use',
+            ),
+            # Simulation for odometry
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
+            ),
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                str(Path(os.path.join(sim_dir)).parent.resolve())
+            ),
+            ExecuteProcess(
+                cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro],
+                output='screen',
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
+                ),
+                launch_arguments={
+                    'use_sim_time': 'True',
+                    'robot_sdf': robot_sdf,
+                    'x_pose': '-2.0',
+                    'y_pose': '-0.5',
+                    'z_pose': '0.01',
+                    'roll': '0.0',
+                    'pitch': '0.0',
+                    'yaw': '0.0',
+                }.items(),
+            ),
+            # No need for localization
+            Node(
+                package='tf2_ros',
+                executable='static_transform_publisher',
+                output='screen',
+                arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
+                parameters=[{'use_sim_time': True}],
+            ),
+            # Need transforms
+            Node(
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                name='robot_state_publisher',
+                output='screen',
+                parameters=[
+                    {'use_sim_time': True, 'robot_description': robot_description}
+                ],
+            ),
+            # Server under test
+            Node(
+                package='nav2_behaviors',
+                executable='behavior_server',
+                name='behavior_server',
+                output='screen',
+                parameters=[configured_params],
+            ),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_navigation',
+                output='screen',
+                parameters=[
+                    {'use_sim_time': True},
+                    {'autostart': True},
+                    {'node_names': ['behavior_server']},
+                ],
+            ),
+        ]
+    )
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(
+            os.getenv('TEST_DIR'), 'drive_tester.py'), '--ros-args', '-p', 'use_sim_time:=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_code = lts.run(ls)
+    return return_code
+
+
+if __name__ == '__main__':
+    sys.exit(main())
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py
deleted file mode 100755
index 8f23ce67b2..0000000000
--- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py
+++ /dev/null
@@ -1,118 +0,0 @@
-#! /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,
-                    'use_composition': 'False',
-                    '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())
diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp
deleted file mode 100644
index b0813c1a25..0000000000
--- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-// 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;
-}
diff --git a/nav2_system_tests/src/behaviors/spin/CMakeLists.txt b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt
index e287b7a534..e9757643fd 100644
--- a/nav2_system_tests/src/behaviors/spin/CMakeLists.txt
+++ b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt
@@ -1,30 +1,8 @@
-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_EXECUTABLE=$<TARGET_FILE:${test_spin_behavior_exec}>
-    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"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior.launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
-  TIMEOUT 180
+  TIMEOUT 60
   ENV
-    TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_behavior_exec}>
-    MAKE_FAKE_COSTMAP=true
+    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
 )
diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp
deleted file mode 100644
index 1e6d57e0f4..0000000000
--- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp
+++ /dev/null
@@ -1,363 +0,0 @@
-// 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,
-  const bool nonblocking_action,
-  const bool cancel_action)
-{
-  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_) {
-    sendFakeCostmap(target_yaw);
-    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;
-  }
-
-  if (!nonblocking_action) {
-    return true;
-  }
-  if (cancel_action) {
-    sleep(2);
-    // cancel the goal
-    auto cancel_response = client_ptr_->async_cancel_goal(goal_handle_future.get());
-    rclcpp::spin_until_future_complete(node_, cancel_response);
-  }
-
-  // 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(3));
-    }
-    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 = node_->now();
-  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 (fabs(angle) > M_PI_2f32) {
-        // fake obstacles in the way so we get failure due to potential collision
-        costmap_val = 253;
-      }
-      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 = node_->now();
-  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 = node_->now();
-  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 = node_->now();
-  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
diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp
deleted file mode 100644
index c7fb6f719d..0000000000
--- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp
+++ /dev/null
@@ -1,110 +0,0 @@
-// 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,
-    bool nonblocking_action = true,
-    bool cancel_action = false);
-
-  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_
diff --git a/nav2_system_tests/src/behaviors/spin/spin_tester.py b/nav2_system_tests/src/behaviors/spin/spin_tester.py
new file mode 100755
index 0000000000..b2d6941987
--- /dev/null
+++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py
@@ -0,0 +1,324 @@
+#! /usr/bin/env python3
+# Copyright 2024 Open Navigation LLC
+#
+# 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 Point32, PolygonStamped
+from nav2_msgs.action import Spin
+from nav2_msgs.msg import Costmap
+from nav2_msgs.srv import ManageLifecycleNodes
+
+import rclpy
+
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+
+
+class SpinTest(Node):
+
+    def __init__(self):
+        super().__init__(node_name='spin_tester', namespace='')
+        self.costmap_qos = QoSProfile(
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+        self.action_client = ActionClient(self, Spin, 'spin')
+        self.costmap_pub = self.create_publisher(
+            Costmap, 'local_costmap/costmap_raw', self.costmap_qos)
+        self.footprint_pub = self.create_publisher(
+            PolygonStamped, 'local_costmap/published_footprint', 10)
+        self.goal_handle = None
+        self.action_result = None
+
+    def sendCommand(self, command):
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        self.info_msg("Waiting for 'spin' action to complete")
+        try:
+            rclpy.spin_until_future_complete(self, self.result_future)
+            status = self.result_future.result().status
+            result = self.result_future.result().result
+            self.action_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 self.action_result.error_code == 0:
+            self.info_msg('Spin was successful!')
+            return True
+        self.info_msg('Spin failed to meet target!')
+        return False
+
+    def sendAndPreemptWithInvertedCommand(self, command):
+        # Send initial goal
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Now preempt it
+        time.sleep(0.5)
+        self.info_msg('Sending preemption request...')
+        command.target_yaw = -command.target_yaw
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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('Preemption rejected')
+            return False
+
+        self.info_msg('Preemption accepted')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Wait for new goal to complete
+        self.info_msg("Waiting for 'spin' action Preemption to complete")
+        try:
+            rclpy.spin_until_future_complete(self, self.result_future)
+            status = self.result_future.result().status
+            result = self.result_future.result().result
+            self.action_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 self.action_result.error_code == 0:
+            self.info_msg('Spin was successful!')
+            return True
+        self.info_msg('Spin failed to meet target!')
+        return False
+
+    def sendAndCancelCommand(self, command):
+        self.info_msg('Sending goal request...')
+        self.goal_future = self.action_client.send_goal_async(command)
+        try:
+            rclpy.spin_until_future_complete(self, self.goal_future)
+            self.goal_handle = self.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')
+        self.result_future = self.goal_handle.get_result_async()
+
+        # Now cancel it
+        time.sleep(0.5)
+        cancel_future = self.goal_handle.cancel_goal_async()
+        rclpy.spin_until_future_complete(self, cancel_future)
+        rclpy.spin_until_future_complete(self, self.result_future)
+        status = self.result_future.result().status
+        if status != GoalStatus.STATUS_CANCELED:
+            self.info_msg(f'Goal failed with status code: {status}')
+            return False
+        else:
+            self.info_msg('Goal was canceled successfully')
+            return True
+
+    def sendFreeCostmap(self):
+        costmap_msg = Costmap()
+        costmap_msg.header.frame_id = 'odom'
+        costmap_msg.header.stamp = self.get_clock().now().to_msg()
+        costmap_msg.metadata.resolution = 0.05
+        costmap_msg.metadata.size_x = 100
+        costmap_msg.metadata.size_y = 100
+        costmap_msg.metadata.origin.position.x = -2.5
+        costmap_msg.metadata.origin.position.y = -2.5
+        costmap_msg.data = [0] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y)
+        self.costmap_pub.publish(costmap_msg)
+
+        footprint_msg = PolygonStamped()
+        footprint_msg.header.frame_id = 'odom'
+        footprint_msg.header.stamp = self.get_clock().now().to_msg()
+        footprint_msg.polygon.points = [
+            Point32(x=0.1, y=0.1, z=0.0),
+            Point32(x=0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=0.1, z=0.0)
+        ]
+        self.footprint_pub.publish(footprint_msg)
+
+    def sendOccupiedCostmap(self):
+        costmap_msg = Costmap()
+        costmap_msg.header.frame_id = 'odom'
+        costmap_msg.header.stamp = self.get_clock().now().to_msg()
+        costmap_msg.metadata.resolution = 0.05
+        costmap_msg.metadata.size_x = 100
+        costmap_msg.metadata.size_y = 100
+        costmap_msg.metadata.origin.position.x = -2.5
+        costmap_msg.metadata.origin.position.y = -2.5
+        costmap_msg.data = [254] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y)
+        self.costmap_pub.publish(costmap_msg)
+
+        footprint_msg = PolygonStamped()
+        footprint_msg.header.frame_id = 'odom'
+        footprint_msg.header.stamp = self.get_clock().now().to_msg()
+        footprint_msg.polygon.points = [
+            Point32(x=0.1, y=0.1, z=0.0),
+            Point32(x=0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=-0.1, z=0.0),
+            Point32(x=-0.1, y=0.1, z=0.0)
+        ]
+        self.footprint_pub.publish(footprint_msg)
+
+    def run(self):
+        while not self.action_client.wait_for_server(timeout_sec=1.0):
+            self.info_msg("'spin' action server not available, waiting...")
+
+        # Test A: Send without valid costmap
+        action_request = Spin.Goal()
+        action_request.target_yaw = 0.349066  # 20 deg
+        action_request.time_allowance = Duration(seconds=5).to_msg()
+        cmd1 = self.sendCommand(action_request)
+
+        if cmd1:
+            self.error_msg('Test A failed: Passed while costmap was not available!')
+            return not cmd1
+        else:
+            self.info_msg('Test A passed')
+
+        # Test B: Send with valid costmap and spin a couple of times
+        self.sendFreeCostmap()
+        time.sleep(1)
+        cmd1 = self.sendCommand(action_request)
+        action_request.target_yaw = -3.49066  # -200 deg
+        cmd2 = self.sendCommand(action_request)
+
+        if not cmd1 or not cmd2:
+            self.error_msg('Test B failed: Failed to spin with valid costmap!')
+            return not cmd1 or not cmd2
+
+        action_request.target_yaw = 0.349066  # 20 deg
+        cmd_preempt = self.sendAndPreemptWithInvertedCommand(action_request)
+        if not cmd_preempt:
+            self.error_msg('Test B failed: Failed to preempt and invert command!')
+            return not cmd_preempt
+
+        cmd_cancel = self.sendAndCancelCommand(action_request)
+        if not cmd_cancel:
+            self.error_msg('Test B failed: Failed to cancel command!')
+            return not cmd_cancel
+        else:
+            self.info_msg('Test B passed')
+
+        # Test C: Send with impossible command in time allowance
+        action_request.time_allowance = Duration(seconds=0.1).to_msg()
+        cmd3 = self.sendCommand(action_request)
+        if cmd3:
+            self.error_msg('Test C failed: Passed while impoossible timing requested!')
+            return not cmd3
+        else:
+            self.info_msg('Test C passed')
+
+        # Test D: Send with lethal costmap and spin
+        action_request.time_allowance = Duration(seconds=5).to_msg()
+        self.sendOccupiedCostmap()
+        time.sleep(1)
+        cmd4 = self.sendCommand(action_request)
+        if cmd4:
+            self.error_msg('Test D failed: Passed while costmap was not lethal!')
+            return not cmd4
+        else:
+            self.info_msg('Test D passed')
+        return True
+
+    def shutdown(self):
+        self.info_msg('Shutting down')
+
+        self.action_client.destroy()
+        self.info_msg('Destroyed backup 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')
+
+    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()
+    time.sleep(10)
+    test = SpinTest()
+    result = test.run()
+    test.shutdown()
+
+    if not result:
+        test.info_msg('Exiting failed')
+        exit(1)
+    else:
+        test.info_msg('Exiting passed')
+        exit(0)
+
+
+if __name__ == '__main__':
+    main()
diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py
similarity index 65%
rename from nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py
rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py
index 492e2d2f8e..cc9a9efa9b 100755
--- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py
+++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py
@@ -23,49 +23,46 @@
 from launch import LaunchService
 from launch.actions import (
     AppendEnvironmentVariable,
+    DeclareLaunchArgument,
     ExecuteProcess,
     IncludeLaunchDescription,
-    SetEnvironmentVariable,
-)
+    SetEnvironmentVariable)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+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')
     sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
-    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
-
+    params_file = LaunchConfiguration('params_file')
     world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
     robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro')
-
     urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
     with open(urdf, 'r') as infp:
         robot_description = infp.read()
 
-    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
-
-    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
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {'use_sim_time': 'True'}
     configured_params = RewrittenYaml(
-        source_file=params_file, root_key='', param_rewrites='', convert_types=True
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True,
     )
 
     return LaunchDescription(
         [
             SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
             SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
-            # Launch gazebo server for simulation
+            DeclareLaunchArgument(
+                'params_file',
+                default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+                description='Full path to the ROS2 parameters file to use',
+            ),
+            # Simulation for odometry
             AppendEnvironmentVariable(
                 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
             ),
@@ -92,6 +89,15 @@ def generate_launch_description():
                     'yaw': '0.0',
                 }.items(),
             ),
+            # No need for localization
+            Node(
+                package='tf2_ros',
+                executable='static_transform_publisher',
+                output='screen',
+                arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
+                parameters=[{'use_sim_time': True}],
+            ),
+            # Need transforms
             Node(
                 package='robot_state_publisher',
                 executable='robot_state_publisher',
@@ -101,18 +107,24 @@ def generate_launch_description():
                     {'use_sim_time': True, 'robot_description': robot_description}
                 ],
             ),
-            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,
-                    'use_composition': 'False',
-                    'autostart': 'True',
-                }.items(),
+            # Server under test
+            Node(
+                package='nav2_behaviors',
+                executable='behavior_server',
+                name='behavior_server',
+                output='screen',
+                parameters=[configured_params],
+            ),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_navigation',
+                output='screen',
+                parameters=[
+                    {'use_sim_time': True},
+                    {'autostart': True},
+                    {'node_names': ['behavior_server']},
+                ],
             ),
         ]
     )
@@ -121,10 +133,11 @@ def generate_launch_description():
 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',
+        cmd=[os.path.join(
+            os.getenv('TEST_DIR'), 'spin_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'],
+        name='tester_node',
+        output='screen',
     )
 
     lts = LaunchTestService()
diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp
deleted file mode 100644
index cf0f97021d..0000000000
--- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp
+++ /dev/null
@@ -1,143 +0,0 @@
-// 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);
-//   }
-// }
-
-TEST_F(SpinBehaviorTestFixture, testSpinPreemption)
-{
-  // Goal
-  float target_yaw = 3.0 * M_PIf32;
-  float tolerance = 0.1;
-  bool nonblocking_action = false;
-  bool success = false;
-
-  // Send the first goal
-  success = spin_recovery_tester->defaultSpinBehaviorTest(
-    target_yaw, tolerance,
-    nonblocking_action);
-  EXPECT_EQ(true, success);
-  // Preempt goal
-  sleep(2);
-  success = false;
-  float prempt_target_yaw = 4.0 * M_PIf32;
-  float preempt_tolerance = 0.1;
-  success = spin_recovery_tester->defaultSpinBehaviorTest(prempt_target_yaw, preempt_tolerance);
-  EXPECT_EQ(false, success);
-}
-
-TEST_F(SpinBehaviorTestFixture, testSpinCancel)
-{
-  // Goal
-  float target_yaw = 4.0 * M_PIf32;
-  float tolerance = 0.1;
-  bool nonblocking_action = true, cancel_action = true, success = false;
-  success = spin_recovery_tester->defaultSpinBehaviorTest(
-    target_yaw, tolerance,
-    nonblocking_action, cancel_action);
-  EXPECT_EQ(false, 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;
-}