diff --git a/README.md b/README.md index 7ff76c4bbd748cb620d357e19dc173172afc828b..5964406191ded67afeecb58c8e2b58029baec218 100644 --- a/README.md +++ b/README.md @@ -236,72 +236,35 @@ like fly-to, move-to, look-at-position and so on are robot/agent specific. Common types are things like seq (S), conc (S), wait, send-signal and so on and are implemented in the system (in lrs_exec). -Here is how the [wait executor](https://gitlab.liu.se/lrs2/lrs_exec/-/blob/main/lrs_exec/executors/wait.py) is implemented in Pyhton: +Here is how the [noop executor](https://gitlab.liu.se/lrs2/lrs_exec/-/blob/main/lrs_exec/executors/noop.py) is implemented in Python: ```python import time import rclpy from rclpy.executors import MultiThreadedExecutor -from lrs_exec.executor import Executor -from lrs_exec.executor_factory import ExecutorFactory +from lrs_exec.ticked_executor import TickedExecutor +from lrs_exec.ticked_executor_factory import TickedExecutorFactory -class WaitExecutor(Executor): +class NoopExecutor(TickedExecutor): def __init__(self, node, id): super().__init__(node, id) - self.set_signals(["$abort", "$pause", "$continue", "$enough", "$finished"]) + + def do_work(self): + return - def start(self): - try: - self.node.get_logger().info("WaitExecutor start") - # self.params.print() - index = 0 - paused = False - duration = self.params.get_value("duration") - if not duration: - self.fail("duration param must be specified") - return - self.node.get_logger().error(f'Duration: {duration}') - finished_flag = False - while not finished_flag and 0.1*index < duration: - if self.have_signal(): - signal = self.pop_signal() - self.node.get_logger().info(f'WaitExecutor got signal: {signal}') - if signal == "$finished": - self.signal_finish() - finished_flag = True - if signal == "$enough": - finished_flag = True - if signal == "$abort": - self.abort_fail("Got abort signal") - finished_flag = True - if signal == "$pause": - paused = True - if signal == "$continue": - self.dji.mission_continue() - paused = False - if not paused: - index += 1 - time.sleep(0.1) - self.node.get_logger().error(f'Wait finished.') - except Exception as exc: - self.node.get_logger().error(f'EXCEPTION start: {type(exc)} - {exc}') - self.fail(f'Exception: {exc}') - ## self.node.get_logger().info("WaitExecutor end") - -class WaitExecutorFactory(ExecutorFactory): +class NoopExecutorFactory(TickedExecutorFactory): def __init__(self): - super().__init__("wait") - + super().__init__("no-op") + def get_executor(self, node, id): - return WaitExecutor(node, id) - + return self.add_executor(NoopExecutor(node, id)) def main(args=None): rclpy.init(args=args) executor = MultiThreadedExecutor(num_threads=8) - node = WaitExecutorFactory() + node = NoopExecutorFactory() executor.add_node(node) print(f"Spinning {__name__}")