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

@@ -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>
+    <name>relais_control</name>
-  <version>0.0.0</version>
+    <version>0.0.0</version>
-  <description>package to control the relais that enables Pixhawk RX and TX communication</description>
+    <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>
+    <test_depend>ament_copyright</test_depend>
-    <build_type>ament_python</build_type>
+    <test_depend>ament_flake8</test_depend>
-  </export>
+    <test_depend>ament_pep257</test_depend>
-</package>
+    <test_depend>python3-pytest</test_depend>
+
+    <export>
+        <build_type>ament_python</build_type>
+    </export>
+</package>

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)