From b779dba01aaf0fc003a49d50079eb3eb0826d73c Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Fri, 26 May 2023 21:38:58 +0200
Subject: [PATCH] add failsafe node

---
 src/drone_services/CMakeLists.txt         |  1 +
 src/drone_services/msg/FailsafeMsg.msg    |  2 ++
 src/drone_services/srv/EnableFailsafe.srv |  4 ++++
 src/failsafe/failsafe/failsafe.py         | 25 +++++++++++++++++++++++
 src/failsafe/package.xml                  |  6 ++++--
 src/failsafe/setup.py                     |  1 +
 6 files changed, 37 insertions(+), 2 deletions(-)
 create mode 100644 src/drone_services/msg/FailsafeMsg.msg
 create mode 100644 src/drone_services/srv/EnableFailsafe.srv

diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt
index d1ee469d..8f126d3b 100644
--- a/src/drone_services/CMakeLists.txt
+++ b/src/drone_services/CMakeLists.txt
@@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/DisarmDrone.srv"
   "srv/ControlRelais.srv"
   "srv/MovePosition.srv"
+  "srv/EnableFailsafe.srv"
   "msg/DroneControlMode.msg"
   "msg/DroneArmStatus.msg"
   "msg/DroneRouteStatus.msg"
diff --git a/src/drone_services/msg/FailsafeMsg.msg b/src/drone_services/msg/FailsafeMsg.msg
new file mode 100644
index 00000000..91277660
--- /dev/null
+++ b/src/drone_services/msg/FailsafeMsg.msg
@@ -0,0 +1,2 @@
+bool enabled
+wstring msg
\ No newline at end of file
diff --git a/src/drone_services/srv/EnableFailsafe.srv b/src/drone_services/srv/EnableFailsafe.srv
new file mode 100644
index 00000000..2243bbf6
--- /dev/null
+++ b/src/drone_services/srv/EnableFailsafe.srv
@@ -0,0 +1,4 @@
+wstring message
+---
+bool enabled
+wstring message
\ No newline at end of file
diff --git a/src/failsafe/failsafe/failsafe.py b/src/failsafe/failsafe/failsafe.py
index 09830e52..483f2fcb 100644
--- a/src/failsafe/failsafe/failsafe.py
+++ b/src/failsafe/failsafe/failsafe.py
@@ -1,9 +1,34 @@
 import rclpy
 from rclpy.node import Node
 
+from drone_services.srv import EnableFailsafe
+from drone_services.msg import FailsafeMsg
 class FailSafe(Node):
     def __init__(self):
         super().init("failsafe")
+        self.failsafe_enabled = False
+        self.failsafe_msg = ""
+        self.get_logger().info("Failsafe node started")
+        # create service on /drone/failsafe topic
+        self.service = self.create_service(
+            EnableFailsafe, "/drone/enable_failsafe", self.failsafe_callback)
+        self.failsafe_publisher = self.create_publisher(FailsafeMsg, "/drone/failsafe", 10)
+    
+    def failsafe_callback(self, request, response):
+        self.failsafe_enabled = True
+        self.failsafe_msg = request.message
+        response.enabled = self.failsafe_enabled
+        response.message = self.failsafe_msg
+        self.get_logger().info("Failsafe triggered")
+        self.publish_failsafe()
+        return response
+    
+    def publish_failsafe(self):
+        msg = FailsafeMsg()
+        msg.enabled = self.failsafe_enabled
+        msg.message = self.failsafe_msg
+        self.failsafe_publisher.publish(msg)
+        self.get_logger().info("Publishing failsafe message")
 
 
 def main(args=None):
diff --git a/src/failsafe/package.xml b/src/failsafe/package.xml
index d262c73d..35c3bca5 100644
--- a/src/failsafe/package.xml
+++ b/src/failsafe/package.xml
@@ -3,9 +3,11 @@
 <package format="3">
   <name>failsafe</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Package to control a failsafe for the drone</description>
   <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache License 2.0</license>
+  <depend>rclpy</depend>
+  <depend>drone_services</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
diff --git a/src/failsafe/setup.py b/src/failsafe/setup.py
index 2dbf1ef6..290e498f 100644
--- a/src/failsafe/setup.py
+++ b/src/failsafe/setup.py
@@ -20,6 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'failsafe = failsafe.failsafe:main',
         ],
     },
 )