From 7935787cecad3a8fedc7245ed04bbedec0593e8f Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Wed, 17 May 2023 11:18:44 +0200
Subject: [PATCH] create relais control node

---
 src/px4_msgs                                  |  2 +-
 src/relais_control/package.xml                |  7 +--
 .../relais_control/relais_controller.py       | 48 +++++++++++++++++++
 src/relais_control/setup.py                   |  1 +
 4 files changed, 54 insertions(+), 4 deletions(-)
 create mode 100644 src/relais_control/relais_control/relais_controller.py

diff --git a/src/px4_msgs b/src/px4_msgs
index ffc3a4cd..b64ef047 160000
--- a/src/px4_msgs
+++ b/src/px4_msgs
@@ -1 +1 @@
-Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266
+Subproject commit b64ef0475c1d44605688f4770899fe453d532be4
diff --git a/src/relais_control/package.xml b/src/relais_control/package.xml
index 648a5861..88e8691e 100644
--- a/src/relais_control/package.xml
+++ b/src/relais_control/package.xml
@@ -3,9 +3,10 @@
 <package format="3">
   <name>relais_control</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
-  <license>TODO: License declaration</license>
+  <description>package to control the relais that enables Pixhawk RX and TX communication</description>
+    <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
+  <license>Apache License 2.0</license>
+    <exec_depend>rclpy</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py
new file mode 100644
index 00000000..7b574746
--- /dev/null
+++ b/src/relais_control/relais_control/relais_controller.py
@@ -0,0 +1,48 @@
+import rclpy
+from rclpy.node import Node
+try:
+    import RPi.GPIO as GPIO
+except RuntimeError:
+    print("Error importing RPi.GPIO!  This is probably because you need superuser privileges.  You can achieve this by using 'sudo' to run your script")
+
+class RelaisController(Node):
+    def __init(self):
+        super().__init__('relais_controller')
+        self.relais1_pin = 17
+        self.relais2_pin = 27
+        self.init_gpio()
+        self.turn_relais_on()
+    
+    def init_gpio(self):
+        GPIO.setwarnings(False)
+
+        self.get_logger().info(GPIO.RPI_INFO)
+
+        GPIO.setmode(GPIO.BCM)
+
+        GPIO.setup(self.relais1_pin, GPIO.OUT)
+        GPIO.setup(self.relais2_pin, GPIO.OUT)
+        self.get_logger().info("GPIO initialized")
+
+    def turn_relais_on(self):
+        GPIO.output(self.relais1_pin, GPIO.HIGH)
+        GPIO.output(self.relais2_pin, GPIO.HIGH)
+        self.get_logger().info("Relais turned on")
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    relais_controller = RelaisController()
+
+    relais_controller.spin()
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    relais_controller.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/src/relais_control/setup.py b/src/relais_control/setup.py
index 858cac94..40560f60 100644
--- a/src/relais_control/setup.py
+++ b/src/relais_control/setup.py
@@ -20,6 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'relais_controller = relais_control.relais_controller:main'
         ],
     },
 )