Created mechanism node

toid_interaction/toid_interaction/interaction_node.py

@@ -0,0 +1,97 @@
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import Empty
+
+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.srv import SendString
+
+class InteracitionNode(Node):
+    step: int = 0
+    def __init__(self):
+        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.get_logger().info("Service 'sequence1' ready.")
+
+    def find_sigma(self):
+        for port_info in list_ports.comports():
+            if port_info.vid == 0x18a6 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):
+            return Empty.Response()
+        okreni(5)
+        zupcanik = ZupcanikAction(self.st_motor_device_name)
+        zupcanik.zupcanik(1, -1010, 25)
+        self.step = 1
+        return response
+
+    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)
+        zidovi.plavi_zid(1)
+
+        okreni_niz(request.text)
+
+        zidovi.plavi_zid(0, TargetPos=150)
+        zidovi.beli_zid(0, TargetPos=450)
+        self.step = 2
+        return response
+
+    def sequence3_cb(self, request, response):
+        if(self.step != 2):
+            return Empty.Response()
+
+        zupcanik = ZupcanikAction(self.st_motor_device_name)
+        zidovi = ZidoviAction(self.st_motor_device_name) 
+
+        zupcanik.zupcanik(1, 1010, 25)
+        zidovi.plavi_zid(0, TargetPos=150)
+        zidovi.beli_zid(0, TargetPos=150)
+        okreni(5)
+        self.step = 0
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = InteracitionNode()
+    rclpy.spin(node)
+
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()