From e16e083dd0b89491bfb2e37280ca9c02f543abec Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Wed, 17 May 2023 11:31:22 +0200
Subject: [PATCH] add service for controlling relais

---
 src/drone_services/CMakeLists.txt             |  1 +
 src/drone_services/srv/ControlRelais.srv      |  5 ++++
 src/relais_control/package.xml                | 26 ++++++++++---------
 .../relais_control/relais_controller.py       | 24 +++++++++++++++++
 4 files changed, 44 insertions(+), 12 deletions(-)
 create mode 100644 src/drone_services/srv/ControlRelais.srv

diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt
index ef491f87..130f0f65 100644
--- a/src/drone_services/CMakeLists.txt
+++ b/src/drone_services/CMakeLists.txt
@@ -28,6 +28,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SetVehicleControl.srv"
   "srv/ArmDrone.srv"
   "srv/DisarmDrone.srv"
+  "srv/ControlRelais.srv"
   "msg/DroneControlMode.msg"
 )
 
diff --git a/src/drone_services/srv/ControlRelais.srv b/src/drone_services/srv/ControlRelais.srv
new file mode 100644
index 00000000..805fcd89
--- /dev/null
+++ b/src/drone_services/srv/ControlRelais.srv
@@ -0,0 +1,5 @@
+#to turn the relais on or off
+bool relais1_on false
+bool relais2_on false
+---
+int8 bits # relais 1 = bit 0, relais 2 is bit 1
\ No newline at end of file
diff --git a/src/relais_control/package.xml b/src/relais_control/package.xml
index 88e8691e..806af7fe 100644
--- a/src/relais_control/package.xml
+++ b/src/relais_control/package.xml
@@ -1,19 +1,21 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>relais_control</name>
-  <version>0.0.0</version>
-  <description>package to control the relais that enables Pixhawk RX and TX communication</description>
+    <name>relais_control</name>
+    <version>0.0.0</version>
+    <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>
+    <license>Apache License 2.0</license>
     <exec_depend>rclpy</exec_depend>
+    <exec_depend>drone_services</exec_depend>
 
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
 
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
+    <test_depend>ament_copyright</test_depend>
+    <test_depend>ament_flake8</test_depend>
+    <test_depend>ament_pep257</test_depend>
+    <test_depend>python3-pytest</test_depend>
+
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>
\ No newline at end of file
diff --git a/src/relais_control/relais_control/relais_controller.py b/src/relais_control/relais_control/relais_controller.py
index 9a9cdb8a..782a4fd3 100644
--- a/src/relais_control/relais_control/relais_controller.py
+++ b/src/relais_control/relais_control/relais_controller.py
@@ -1,13 +1,17 @@
 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")
 
+from drone_services.srv import ControlRelais
 class RelaisController(Node):
     def __init__(self):
         super().__init__('relais_controller')
+        self.srv = self.create_service(ControlRelais, 'control_relais', self.control_relais_callback)
+
         self.relais1_pin = 17
         self.relais2_pin = 27
         self.init_gpio()
@@ -29,6 +33,26 @@ class RelaisController(Node):
         GPIO.output(self.relais2_pin, GPIO.HIGH)
         self.get_logger().info("Relais turned on")
 
+    def control_relais_callback(self, request, response):
+        if request.relais1:
+            GPIO.output(self.relais1_pin, GPIO.HIGH)
+        else:
+            GPIO.output(self.relais1_pin, GPIO.LOW)
+        if request.relais2:
+            GPIO.output(self.relais2_pin, GPIO.HIGH)
+        else:
+            GPIO.output(self.relais2_pin, GPIO.LOW)
+        
+        if GPIO.output(17) == GPIO.LOW:
+            response.bits = response.bits & 0
+        else:
+            response.bits = response.bits | 1
+
+        if GPIO.output(27) == GPIO.LOW:
+            response.bits = response.bits & ~(1 << 1)
+        else:
+            response.bits = response.bits | (1 << 1)
+        return response
 
 def main(args=None):
     rclpy.init(args=args)