@@ -27,122 +27,168 @@ impl WaitSetImpl {
2727 pub fn wait ( & self , timeout : & rmw_time_t ) -> bool {
2828 use std:: time:: Duration ;
2929
30+ tracing:: debug!( "[WAIT] wait() called with timeout: {}s {}ns" , timeout. sec, timeout. nsec) ;
31+
3032 // If timeout is zero, check ready immediately and return
3133 if timeout. sec == 0 && timeout. nsec == 0 {
32- return self . check_ready ( ) ;
34+ let ready = self . check_ready ( ) ;
35+ tracing:: debug!( "[WAIT] Zero timeout, check_ready returned: {}" , ready) ;
36+ return ready;
3337 }
3438
3539 // Calculate timeout duration
3640 let timeout_duration = if timeout. sec == u64:: MAX {
41+ tracing:: debug!( "[WAIT] Infinite timeout" ) ;
3742 None // Infinite wait
3843 } else {
39- Some ( Duration :: from_secs ( timeout. sec ) + Duration :: from_nanos ( timeout. nsec ) )
44+ let dur = Duration :: from_secs ( timeout. sec ) + Duration :: from_nanos ( timeout. nsec ) ;
45+ tracing:: debug!( "[WAIT] Finite timeout: {:?}" , dur) ;
46+ Some ( dur)
4047 } ;
4148
49+ // CRITICAL: Check if anything is already ready BEFORE waiting
50+ // This handles the case where trigger() happened before wait()
51+ let already_ready = self . check_ready ( ) ;
52+ tracing:: debug!( "[WAIT] Pre-wait check_ready: {}" , already_ready) ;
53+ if already_ready {
54+ tracing:: debug!( "[WAIT] Already ready, returning immediately" ) ;
55+ return true ;
56+ }
57+
4258 // Use the notifier's condition variable for efficient waiting
59+ tracing:: debug!( "[WAIT] Acquiring mutex lock..." ) ;
4360 let mut mutex_guard = self . notifier . mutex . lock ( ) ;
61+ tracing:: debug!( "[WAIT] Mutex locked, entering wait loop" ) ;
4462
45- // Always wait (at least try to) - this prevents busy loops when data is already present
46- // We'll check ready status after waiting or timing out
63+ // Now wait for notification
4764 loop {
4865 if let Some ( dur) = timeout_duration {
66+ tracing:: debug!( "[WAIT] Calling wait_for({:?})" , dur) ;
4967 let wait_result = self . notifier . cv . wait_for ( & mut mutex_guard, dur) ;
68+ tracing:: debug!( "[WAIT] wait_for returned, timed_out: {}" , wait_result. timed_out( ) ) ;
5069
5170 // After wait (notification or timeout), check if anything is ready
5271 let is_ready = self . check_ready ( ) ;
72+ tracing:: debug!( "[WAIT] Post-wait check_ready: {}" , is_ready) ;
5373
5474 if is_ready {
75+ tracing:: debug!( "[WAIT] Ready after wait, returning true" ) ;
5576 return true ;
5677 }
5778
5879 // Nothing ready
5980 if wait_result. timed_out ( ) {
81+ tracing:: debug!( "[WAIT] Timed out, returning false" ) ;
6082 return false ;
6183 }
6284
85+ tracing:: debug!( "[WAIT] Spurious wakeup, looping..." ) ;
6386 // Spurious wakeup - nothing ready yet, loop and wait again
6487 } else {
6588 // Infinite wait
89+ tracing:: debug!( "[WAIT] Calling wait (infinite)" ) ;
6690 self . notifier . cv . wait ( & mut mutex_guard) ;
91+ tracing:: debug!( "[WAIT] Woke up from infinite wait" ) ;
6792
6893 // Check if anything is ready after waking
69- if self . check_ready ( ) {
94+ let is_ready = self . check_ready ( ) ;
95+ tracing:: debug!( "[WAIT] Post-wake check_ready: {}" , is_ready) ;
96+ if is_ready {
97+ tracing:: debug!( "[WAIT] Ready after wake, returning true" ) ;
7098 return true ;
7199 }
100+ tracing:: debug!( "[WAIT] Not ready after wake, looping..." ) ;
72101 // If notified but nothing ready yet, loop and wait again
73102 }
74103 }
75104 }
76105
77106 fn check_ready ( & self ) -> bool {
107+ tracing:: debug!( "[WAIT] check_ready: {} subs, {} gcs, {} srvs, {} clients, {} events" ,
108+ self . subscriptions. len( ) , self . guard_conditions. len( ) , self . services. len( ) ,
109+ self . clients. len( ) , self . events. len( ) ) ;
110+
78111 // Check subscriptions
79- for sub_impl_ptr in & self . subscriptions {
112+ for ( i , sub_impl_ptr) in self . subscriptions . iter ( ) . enumerate ( ) {
80113 if sub_impl_ptr. is_null ( ) {
81114 continue ;
82115 }
83116 unsafe {
84117 let sub_impl = & * ( ( * sub_impl_ptr) as * const _ as * const crate :: pubsub:: SubscriptionImpl ) ;
85- if sub_impl. is_ready ( ) {
118+ let ready = sub_impl. is_ready ( ) ;
119+ tracing:: debug!( "[WAIT] Subscription {}: ready={}" , i, ready) ;
120+ if ready {
86121 return true ;
87122 }
88123 }
89124 }
90125
91126 // Check guard conditions
92- for gc_impl_ptr in & self . guard_conditions {
127+ for ( i , gc_impl_ptr) in self . guard_conditions . iter ( ) . enumerate ( ) {
93128 if gc_impl_ptr. is_null ( ) {
129+ tracing:: debug!( "[WAIT] Guard condition {}: NULL" , i) ;
94130 continue ;
95131 }
96132 unsafe {
97133 let gc_impl = & * ( * gc_impl_ptr as * const _ as * const crate :: guard_condition:: GuardConditionImpl ) ;
98- if gc_impl. is_ready ( ) {
134+ let ready = gc_impl. is_ready ( ) ;
135+ tracing:: debug!( "[WAIT] Guard condition {}: ready={}" , i, ready) ;
136+ if ready {
137+ tracing:: debug!( "[WAIT] Guard condition {} is ready! Returning true" , i) ;
99138 return true ;
100139 }
101140 }
102141 }
103142
104143 // Check services
105- for srv_impl_ptr in & self . services {
144+ for ( i , srv_impl_ptr) in self . services . iter ( ) . enumerate ( ) {
106145 if srv_impl_ptr. is_null ( ) {
107146 continue ;
108147 }
109148 unsafe {
110149 let srv_impl = & * ( * srv_impl_ptr as * const _ as * const crate :: service:: ServiceImpl ) ;
111- if srv_impl. is_ready ( ) {
150+ let ready = srv_impl. is_ready ( ) ;
151+ tracing:: debug!( "[WAIT] Service {}: ready={}" , i, ready) ;
152+ if ready {
112153 return true ;
113154 }
114155 }
115156 }
116157
117158 // Check clients
118- for cli_impl_ptr in & self . clients {
159+ for ( i , cli_impl_ptr) in self . clients . iter ( ) . enumerate ( ) {
119160 if cli_impl_ptr. is_null ( ) {
120161 continue ;
121162 }
122163 unsafe {
123164 let cli_impl = & * ( * cli_impl_ptr as * const _ as * const crate :: service:: ClientImpl ) ;
124- if cli_impl. is_ready ( ) {
165+ let ready = cli_impl. is_ready ( ) ;
166+ tracing:: debug!( "[WAIT] Client {}: ready={}" , i, ready) ;
167+ if ready {
125168 return true ;
126169 }
127170 }
128171 }
129172
130173 // Check events
131- for event_ptr in & self . events {
174+ for ( i , event_ptr) in self . events . iter ( ) . enumerate ( ) {
132175 if event_ptr. is_null ( ) {
133176 continue ;
134177 }
135178 unsafe {
136179 let event = & * ( * event_ptr) ;
137180 if !event. data . is_null ( ) {
138181 let event_handle = & * ( event. data as * const ros_z:: event:: RmEventHandle ) ;
139- if event_handle. is_ready ( ) {
182+ let ready = event_handle. is_ready ( ) ;
183+ tracing:: debug!( "[WAIT] Event {}: ready={}" , i, ready) ;
184+ if ready {
140185 return true ;
141186 }
142187 }
143188 }
144189 }
145190
191+ tracing:: debug!( "[WAIT] check_ready: nothing ready" ) ;
146192 false
147193 }
148194}
0 commit comments