diff --git a/src/failsafe/test/test_copyright.py b/src/failsafe/test/test_copyright.py
deleted file mode 100644
index cc8ff03f..00000000
--- a/src/failsafe/test/test_copyright.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# 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.
-
-from ament_copyright.main import main
-import pytest
-
-
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'
diff --git a/src/failsafe/test/test_failsafe.py b/src/failsafe/test/test_failsafe.py
new file mode 100644
index 00000000..49585e8e
--- /dev/null
+++ b/src/failsafe/test/test_failsafe.py
@@ -0,0 +1,83 @@
+import os
+import sys
+import unittest
+import time
+
+import launch
+import launch_ros
+import launch_ros.actions
+import launch_testing.actions
+
+import rclpy
+
+from drone_services.srv import EnableFailsafe
+from drone_services.msg import FailsafeMsg
+
+# launch node
+def generate_test_description():
+    file_path = os.path.dirname(__file__)
+    failsafe_node = launch_ros.actions.Node(
+        executable=sys.executable,
+        arguments=[os.path.join(file_path, '..', 'failsafe', 'failsafe.py')],
+        additional_env={'PYTHONUNBUFFERED': '1'}
+    )
+
+    return (
+        launch.LaunchDescription([
+            failsafe_node,
+            launch_testing.actions.ReadyToTest(),
+        ]),
+        {
+            'failsafe_node': failsafe_node,
+        }
+    )
+
+class FailsafeUnitTest(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+    
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+    
+    def setUp(self):
+        self.node = rclpy.create_node('test_failsafe')
+        self.service_called = False
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    def service_call_callback(self,future):
+        self.assertIsNotNone(future.result())
+        self.assertTrue(future.result().enabled)
+        self.assertEqual(future.result().message, "test")
+        self.service_called = True
+    
+    def test_failsafe_node_enables(self,failsafe_node,proc_output):
+        failsafe_msgs = []
+        failsafe_subscription = self.node.create_subscription(FailsafeMsg, "/drone/failsafe", lambda msg: failsafe_msgs.append(msg), 10)
+        failsafe_client = self.node.create_client(EnableFailsafe, "/drone/enable_failsafe")
+        while not failsafe_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info("service not available, waiting again...")
+        request = EnableFailsafe.Request()
+        request.message = "test"
+
+        try:
+            end_time = time.time() + 10.0
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                if (not self.service_called):
+                    future = failsafe_client.call_async(request)
+                    future.add_done_callback(self.service_call_callback)
+                if (len(failsafe_msgs) > 0):
+                    break
+            self.assertTrue(failsafe_msgs[0].enabled)
+            self.assertEqual(failsafe_msgs[0].msg, "test")
+        finally:
+            self.node.destroy_subscription(failsafe_subscription)
+            self.node.destroy_client(failsafe_client)
+                
+
+
diff --git a/src/failsafe/test/test_flake8.py b/src/failsafe/test/test_flake8.py
deleted file mode 100644
index 27ee1078..00000000
--- a/src/failsafe/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# 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.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)
diff --git a/src/failsafe/test/test_pep257.py b/src/failsafe/test/test_pep257.py
deleted file mode 100644
index b234a384..00000000
--- a/src/failsafe/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# 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.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'