2222
2323from __future__ import annotations
2424
25+ import asyncio
2526import math
2627from pathlib import Path
27- import threading
2828import time
2929
3030import lcm as lcmlib
3131
32- from dimos .constants import DEFAULT_THREAD_JOIN_TIMEOUT
3332from dimos .core .coordination .blueprints import Blueprint
3433from dimos .core .coordination .module_coordinator import ModuleCoordinator
3534from dimos .msgs .geometry_msgs .PointStamped import PointStamped
@@ -76,13 +75,11 @@ def _clear_precomputed_paths() -> None:
7675 path .unlink (missing_ok = True )
7776
7877
79- def run_cross_wall_test (blueprint : Blueprint , * , label : str , max_z : float | None = None ) -> None :
80- """Build the coordinator, drive the cross-wall waypoint sequence, tear down."""
78+ async def _run_cross_wall_test (blueprint : Blueprint , * , label : str , max_z : float | None ) -> None :
8179 _clear_precomputed_paths ()
8280
8381 coordinator = ModuleCoordinator .build (blueprint )
8482
85- lock = threading .Lock ()
8683 odom_count = 0
8784 robot_x = 0.0
8885 robot_y = 0.0
@@ -94,52 +91,43 @@ def run_cross_wall_test(blueprint: Blueprint, *, label: str, max_z: float | None
9491 def _odom_handler (_channel : str , data : bytes ) -> None :
9592 nonlocal odom_count , robot_x , robot_y , robot_z , max_z_seen
9693 msg = Odometry .lcm_decode (data )
97- with lock :
98- odom_count += 1
99- robot_x = msg .x
100- robot_y = msg .y
101- robot_z = msg .pose .position .z
102- if robot_z > max_z_seen :
103- max_z_seen = robot_z
94+ odom_count += 1
95+ robot_x = msg .x
96+ robot_y = msg .y
97+ robot_z = msg .pose .position .z
98+ if robot_z > max_z_seen :
99+ max_z_seen = robot_z
104100
105101 subscription = lcm .subscribe (ODOM_TOPIC , _odom_handler )
106102
107- lcm_stop = threading .Event ()
103+ loop = asyncio .get_running_loop ()
104+ lcm_fd = lcm .fileno ()
108105
109- def _lcm_loop () -> None :
110- while not lcm_stop .is_set ():
111- try :
112- lcm .handle_timeout (100 )
113- except Exception :
114- # Don't spin forever waiting on odom that will never arrive.
115- logger .exception ("LCM handle_timeout failed; stopping loop" )
116- lcm_stop .set ()
117- return
106+ def _on_lcm_readable () -> None :
107+ try :
108+ lcm .handle ()
109+ except Exception :
110+ logger .exception ("LCM handle failed; removing reader to stop further polling" )
111+ loop .remove_reader (lcm_fd )
118112
119- lcm_thread = threading .Thread (target = _lcm_loop , daemon = True )
120- lcm_thread .start ()
113+ loop .add_reader (lcm_fd , _on_lcm_readable )
121114
122115 try :
123116 logger .info (f"[{ label } ] Blueprint started, waiting for odom…" )
124117
125118 deadline = time .monotonic () + ODOM_WAIT_SEC
126- while time .monotonic () < deadline :
127- with lock :
128- if odom_count > 0 :
129- break
130- time .sleep (0.5 )
119+ while time .monotonic () < deadline and odom_count == 0 :
120+ await asyncio .sleep (0.5 )
131121
132- with lock :
133- assert odom_count > 0 , f"No odometry received after { ODOM_WAIT_SEC } s — sim not running?"
134- initial_x , initial_y = robot_x , robot_y
122+ assert odom_count > 0 , f"No odometry received after { ODOM_WAIT_SEC } s — sim not running?"
123+ initial_x , initial_y = robot_x , robot_y
135124
136125 logger .info (f"[{ label } ] Odom online. Robot at ({ initial_x :.2f} , { initial_y :.2f} )" )
137126 logger .info (f"[{ label } ] Warming up for { WARMUP_SEC } s…" )
138- time .sleep (WARMUP_SEC )
127+ await asyncio .sleep (WARMUP_SEC )
139128
140129 for name , goal_x , goal_y , goal_z , timeout_sec , threshold in CROSS_WALL_WAYPOINTS :
141- with lock :
142- start_x , start_y = robot_x , robot_y
130+ start_x , start_y = robot_x , robot_y
143131
144132 logger .info (
145133 f"[{ label } ] === { name } : goal ({ goal_x } , { goal_y } ) | "
@@ -156,10 +144,9 @@ def _lcm_loop() -> None:
156144 current_x , current_y = start_x , start_y
157145 distance = _distance (current_x , current_y , goal_x , goal_y )
158146 while True :
159- with lock :
160- current_x , current_y = robot_x , robot_y
161- current_z = robot_z
162- current_max_z = max_z_seen
147+ current_x , current_y = robot_x , robot_y
148+ current_z = robot_z
149+ current_max_z = max_z_seen
163150
164151 if max_z is not None :
165152 assert current_z <= max_z , (
@@ -176,24 +163,25 @@ def _lcm_loop() -> None:
176163 break
177164 if elapsed >= timeout_sec :
178165 break
179- time .sleep (GOAL_POLL_INTERVAL_SEC )
166+ await asyncio .sleep (GOAL_POLL_INTERVAL_SEC )
180167
181168 assert reached , (
182169 f"{ name } : robot did not reach ({ goal_x } , { goal_y } ) within { timeout_sec } s. "
183170 f"Final pos=({ current_x :.2f} , { current_y :.2f} ), dist={ distance :.2f} m"
184171 )
185172
186173 if max_z is not None :
187- with lock :
188- final_max_z = max_z_seen
189- assert final_max_z <= max_z , (
190- f"Robot z peaked at { final_max_z :.2f} m during the run "
174+ assert max_z_seen <= max_z , (
175+ f"Robot z peaked at { max_z_seen :.2f} m during the run "
191176 f"(limit { max_z } m) — went through the ceiling"
192177 )
193178
194179 finally :
195- lcm_stop .set ()
196- lcm_thread .join (timeout = DEFAULT_THREAD_JOIN_TIMEOUT )
197- assert not lcm_thread .is_alive (), "LCM loop thread didn't exit cleanly"
180+ loop .remove_reader (lcm_fd )
198181 lcm .unsubscribe (subscription )
199182 coordinator .stop ()
183+
184+
185+ def run_cross_wall_test (blueprint : Blueprint , * , label : str , max_z : float | None = None ) -> None :
186+ """Build the coordinator, drive the cross-wall waypoint sequence, tear down."""
187+ asyncio .run (_run_cross_wall_test (blueprint , label = label , max_z = max_z ))
0 commit comments