From 63f707d3554260bad6d7cbb9a5e648502e8e8060 Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Tue, 17 Jan 2023 12:11:36 +0200 Subject: [PATCH] Improve homing --- src/axidraw_controller/src/py/axidraw_serial.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/axidraw_controller/src/py/axidraw_serial.py b/src/axidraw_controller/src/py/axidraw_serial.py index b0b1210..d137e2c 100644 --- a/src/axidraw_controller/src/py/axidraw_serial.py +++ b/src/axidraw_controller/src/py/axidraw_serial.py @@ -69,6 +69,7 @@ class AxidrawSerial(Node): return response def go_home(self): + self.wait_ready() self.status["motion"] = "busy" if self.status["serial"] == "ready": self.ad.moveto(0,0) @@ -80,6 +81,11 @@ class AxidrawSerial(Node): def set_ready(self): self.status["motion"] = "ready" + def wait_ready(self): + rate = node.create_rate(2) #2Hz + while self.status["motion"] != "ready": + rate.sleep() + def move_callback(self, msg): self.set_busy() @@ -119,8 +125,8 @@ def main(args=None): axidraw_serial = AxidrawSerial() + rclpy.on_shutdown(axidraw_serial.go_home()) rclpy.spin(axidraw_serial) - axidraw_serial.go_home() rclpy.shutdown()