Work on ROS2

lrs_exec/executors/wait.py

@@ -11,7 +11,7 @@ class WaitExecutor(Executor):
 
     def start(self):
         try:
-            self.lprint("WaitExecutor start")
+            self.node.get_logger().info("WaitExecutor start")
             # self.params.print()
             index = 0
             paused = False
@@ -23,7 +23,7 @@ class WaitExecutor(Executor):
             while not finished_flag and 0.1*index < duration:
                 if self.have_signal():
                     signal = self.pop_signal()
-                    self.lprint("WaitExecutor got signal:", signal)
+                    self.node.get_logger().info(f'WaitExecutor got signal: {signal}')
                     if signal == "$enough":
                         finished_flag = True
                     if signal == "$abort":
@@ -37,10 +37,9 @@ class WaitExecutor(Executor):
                     index += 1
                 time.sleep(0.1)
         except Exception as exc:
-            print(type(exc))
-            print(exc)
+            self.node.get_logger().error(f'EXCEPTION start: {type(exc)} - {exc}')            
             self.fail(f'Exception: {exc}')
-        self.lprint("WaitExecutor end")
+        self.node.get_logger().info("WaitExecutor end")
             
 class WaitExecutorFactory(ExecutorFactory):
     def __init__(self):