From 112687863365d1f4e77e3195d2f9b4fe09c5637a Mon Sep 17 00:00:00 2001
From: Tommy Persson <tommy.persson@liu.se>
Date: Sun, 30 Mar 2025 22:38:44 +0200
Subject: [PATCH] Work on lrs_ardupilot debugging
---
lrs_exec/executor.py | 15 ++++++++++++---
lrs_exec/executor_factory.py | 2 +-
lrs_exec/ticked_executor.py | 11 +++++++----
lrs_exec/ticked_executor_factory.py | 2 +-
lrs_exec/tree_executor.py | 2 +-
5 files changed, 22 insertions(+), 10 deletions(-)
diff --git a/lrs_exec/executor.py b/lrs_exec/executor.py
index 71516c6..5104dcf 100644
--- a/lrs_exec/executor.py
+++ b/lrs_exec/executor.py
@@ -466,6 +466,7 @@ class Executor():
self.node.get_logger().error(f'do_before_work - pre-work conditions failed: {self.id}')
return False
+ self.node.get_logger().info(f'do_before_work - RETURN true: {self.id}')
return True
def wait_for_approved(self):
@@ -624,9 +625,13 @@ class Executor():
return False
def ticked_wait_for_prework_conditions(self):
+ ## self.node.get_logger().error(f'ticked_wait_for_prework_conditions')
if self.ticked_executor_wait():
+ ## self.node.get_logger().error(f'ticked_wait_for_prework_conditions.SUCCESS')
self.executor_wait_finished = True
self.set_executing_flag(True)
+ else:
+ self.node.get_logger().error(f'ticked_wait_for_prework_conditions.FAIL')
def unlock_resources(self):
res = True
@@ -672,21 +677,25 @@ class Executor():
return self.normal_exit_flag
def basic_do_before_work(self):
+ self.node.get_logger().info(f'basic_do_before_work - ENTER true: {self.id}')
if not self.do_before_work():
self.failed = True
self.finished = True
- self.node.get_logger().error(f'Executor basic_start: do_before_work failed')
+ ## self.node.get_logger().error(f'Executor basic_start: do_before_work failed')
self.basic_start_exited = True
+ self.node.get_logger().info(f'basic_do_before_work - RETURN false: {self.id}')
return False
+ self.node.get_logger().info(f'basic_do_before_work - RETURN true: {self.id}')
return True
def basic_start(self):
- self.node.get_logger().error(f'Executor basic_start: {self.id}')
+ ## self.node.get_logger().error(f'Executor basic_start: {self.id}')
if not self.basic_do_before_work():
+ self.node.get_logger().error(f'Executor basic_do_before_work failed')
return
- self.node.get_logger().error(f'Executor basic_start before callin start: {self.id}')
+ ## self.node.get_logger().error(f'Executor basic_start before callin start: {self.id}')
self.start()
if self.normal_exit():
diff --git a/lrs_exec/executor_factory.py b/lrs_exec/executor_factory.py
index 08f9826..fb529b8 100644
--- a/lrs_exec/executor_factory.py
+++ b/lrs_exec/executor_factory.py
@@ -226,7 +226,7 @@ class ExecutorFactory(Node):
self.get_logger().info(f"Call basic start on {type(self.executor_map[id])}")
if self.executor_map[id].use_ticking:
- self.get_logger().info(f'Use ticking, no threads')
+ self.get_logger().info(f'executor_factory: Use ticking, no threads')
if self.executor_map[id].basic_do_before_work():
self.executor_map[id].start()
else:
diff --git a/lrs_exec/ticked_executor.py b/lrs_exec/ticked_executor.py
index 9df7e69..ac0996a 100644
--- a/lrs_exec/ticked_executor.py
+++ b/lrs_exec/ticked_executor.py
@@ -19,6 +19,7 @@ class TickedExecutor(Executor):
return
def tick(self):
+ ## self.node.get_logger().info(f'ticked_executor tick: {self.inside_tick} {self.started_flag}')
if self.inside_tick:
return
#if self.finished:
@@ -27,7 +28,7 @@ class TickedExecutor(Executor):
return
self.inside_tick = True
try:
- # self.node.get_logger().error(f'Tick!!! {self.tni.type} - {self.id}')
+ ## self.node.get_logger().error(f'Tick!!! {self.tni.type} - {self.id}')
if self.tick_finished and not self.first_tick:
self.node.get_logger().info(f'Tick tick_finished: {self.tni.type}')
@@ -41,7 +42,7 @@ class TickedExecutor(Executor):
self.started_flag = False
self.inside_tick = False
return
-
+
if not self.executor_wait_finished:
self.ticked_wait_for_prework_conditions()
if self.executor_wait_failed:
@@ -59,19 +60,21 @@ class TickedExecutor(Executor):
self.do_work_finished()
self.inside_tick = False
return
+
if self.executor_wait_finished and self.tni.enough_when_starting:
self.node.get_logger().info(f'Tick ENOUGH: {self.executor_wait_finished}')
self.do_work_finished()
self.inside_tick = False
return
+
if self.first_tick:
- self.node.get_logger().info(f'FIRST Tick - {self.id}')
+ ## self.node.get_logger().info(f'FIRST Tick - {self.id}')
self.do_work_begin()
if not self.first_tick:
- # self.node.get_logger().error(f'NOT FIRST Tick - {self.id}')
+ ## self.node.get_logger().error(f'NOT FIRST Tick - {self.id}')
if self.blocking:
self.tick_finished = True
self.do_work()
diff --git a/lrs_exec/ticked_executor_factory.py b/lrs_exec/ticked_executor_factory.py
index e1ba81c..53b4ad7 100644
--- a/lrs_exec/ticked_executor_factory.py
+++ b/lrs_exec/ticked_executor_factory.py
@@ -23,7 +23,7 @@ class TickedExecutorFactory(ExecutorFactory):
self.executors_lock.acquire()
self.executors.append(obj)
self.executors_lock.release()
- self.get_logger().error(f'add_executor: {self.type}')
+ self.get_logger().info(f'add_executor: {self.type}')
return obj
def tick(self):
diff --git a/lrs_exec/tree_executor.py b/lrs_exec/tree_executor.py
index ce88870..84e6a62 100644
--- a/lrs_exec/tree_executor.py
+++ b/lrs_exec/tree_executor.py
@@ -459,7 +459,7 @@ class TreeExecutor(Node):
jobjstr = json.dumps(jobj, sort_keys=True, indent=4, separators=(',', ': '))
self.get_logger().info(f'traverse_start_non_terminal: {jobjstr}')
- self.get_logger().info(f'traverse_start_non_terminal: {jobj["execution_ns"]} - {self.get_namespace()}')
+ ## self.get_logger().info(f'traverse_start_non_terminal: {jobj["execution_ns"]} - {self.get_namespace()}')
if jobj["execution_ns"] != self.get_namespace():
## self.get_logger().info(f'traverse_start_non_terminal: RETURN FALSE')
--
GitLab