Added start plug action server

toid_interaction/package.xml

@@ -13,6 +13,7 @@
   <test_depend>python3-pytest</test_depend>
 
   <depend>python3-serial</depend>
+  <depend>python3-gpiozero</depend>
 
   <export>
     <build_type>ament_python</build_type>

toid_interaction/setup.py

@@ -25,7 +25,9 @@ setup(
     entry_points={
         'console_scripts': [
             'sequence = toid_interaction.mechanism.sekvenca_2026:main',
-            'node = toid_interaction.interaction_node:main'
+            'node = toid_interaction.interaction_node:main',
+            'cam_calib = toid_interaction.camera:main',
+            'cam_calib1 = toid_interaction.camera1:main'
         ],
     },
 )

toid_interaction/toid_interaction/interaction_node.py

@@ -2,52 +2,60 @@ import rclpy
 from rclpy.node import Node
 from std_srvs.srv import Empty
 
+from gpiozero import Button, OutputDevice
+
 from serial.tools import list_ports
 
 from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz
 from toid_interaction.mechanism.zidovi_load import ZidoviAction
 from toid_interaction.mechanism.zupcanik import ZupcanikAction
+from toid_msgs.action import EmptyAction
 from toid_msgs.srv import SendString
 
+from rclpy.action.server import ServerGoalHandle
+from rclpy.action.server import ActionServer
+import asyncio
+
+
 class InteracitionNode(Node):
     step: int = 0
+    btn_: Button
+    output_pin_: OutputDevice
+    start_pin_action_: ActionServer
+
     def __init__(self):
-        super().__init__('ToidInteractionNode')
+        super().__init__("ToidInteractionNode")
 
         self.find_sigma()
 
-        self.srv = self.create_service(
-            Empty,
-            '/sequence1',
-            self.sequence1_cb
-        )
-
-        self.srv = self.create_service(
-            SendString,
-            '/sequence2',
-            self.sequence2_cb
-        )
-
-        self.srv = self.create_service(
-            Empty,
-            '/sequence3',
-            self.sequence3_cb
-        )
-
-
-
+        self.srv = self.create_service(Empty, "/sequence1", self.sequence1_cb)
         self.get_logger().info("Service 'sequence1' ready.")
 
+        self.srv = self.create_service(SendString, "/sequence2", self.sequence2_cb)
+        self.get_logger().info("Service 'sequence2' ready.")
+
+        self.srv = self.create_service(Empty, "/sequence3", self.sequence3_cb)
+        self.get_logger().info("Service 'sequence3' ready.")
+
+        self.btn_ = Button(17)
+        self.output_pin_ = OutputDevice(27)
+
+        self.start_pin_action_ = ActionServer(
+            self, EmptyAction, "/start_plug", execute_callback=self.start_plug_action_cb
+        )
+        self.get_logger().info("Action 'start_plug' ready.")
+
+
     def find_sigma(self):
         for port_info in list_ports.comports():
-            if port_info.vid == 0x1a86 and port_info.pid == 0x55d3:
+            if port_info.vid == 0x1A86 and port_info.pid == 0x55D3:
                 break
 
         print(port_info.device)
         self.st_motor_device_name = port_info.device
 
     def sequence1_cb(self, request, response):
-        if(self.step != 0):
+        if self.step != 0:
             return Empty.Response()
         okreni(5)
         zupcanik = ZupcanikAction(self.st_motor_device_name)
@@ -55,8 +63,8 @@ class InteracitionNode(Node):
         self.step = 1
         return response
 
-    def sequence2_cb(self, request : SendString.Request, response : SendString.Response):
-        if(self.step != 1):
+    def sequence2_cb(self, request: SendString.Request, response: SendString.Response):
+        if self.step != 1:
             return Empty.Response()
         zidovi = ZidoviAction(self.st_motor_device_name)
         zidovi.beli_zid(1)
@@ -70,11 +78,11 @@ class InteracitionNode(Node):
         return response
 
     def sequence3_cb(self, request, response):
-        if(self.step != 2):
+        if self.step != 2:
             return Empty.Response()
 
         zupcanik = ZupcanikAction(self.st_motor_device_name)
-        zidovi = ZidoviAction(self.st_motor_device_name) 
+        zidovi = ZidoviAction(self.st_motor_device_name)
 
         zupcanik.zupcanik(1, 1010, 25)
         zidovi.plavi_zid(0, TargetPos=150)
@@ -83,6 +91,14 @@ class InteracitionNode(Node):
         self.step = 0
         return response
 
+    async def start_plug_action_cb(self, goal_handle: ServerGoalHandle):
+        while not self.btn_.is_active:
+            await asyncio.sleep(0.1)
+        while self.btn_.is_active:
+            await asyncio.sleep(0.1)
+        goal_handle.succeed()
+        return EmptyAction.Result()
+
 
 def main(args=None):
     rclpy.init(args=args)
@@ -93,5 +109,5 @@ def main(args=None):
     rclpy.shutdown()
 
 
-if __name__ == '__main__':
-    main()
+if __name__ == "__main__":
+    main()