add failsafe node

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"

src/drone_services/msg/FailsafeMsg.msg

@@ -0,0 +1,2 @@
+bool enabled
+wstring msg

src/drone_services/srv/EnableFailsafe.srv

@@ -0,0 +1,4 @@
+wstring message
+---
+bool enabled
+wstring message

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):

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>

src/failsafe/setup.py

@@ -20,6 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'failsafe = failsafe.failsafe:main',
         ],
     },
 )