add service for controlling relais

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

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

src/relais_control/package.xml

@@ -7,6 +7,8 @@
     <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
     <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>

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)