97 lines
2.4 KiB
Python
97 lines
2.4 KiB
Python
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 == 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):
|
|
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() |