@@ -18,7 +18,7 @@ pub struct TimeSource {
18
18
// TODO(luca) Update this with parameter callbacks for use_sim_time
19
19
_ros_time_active : Arc < Mutex < bool > > ,
20
20
_clock_subscription : Option < Arc < Subscription < ClockMsg > > > ,
21
- // TODO(luca) add last time message for newly attached clocks initialization
21
+ _last_time_msg : Arc < Mutex < Option < ClockMsg > > > ,
22
22
}
23
23
24
24
pub struct TimeSourceBuilder {
@@ -56,6 +56,7 @@ impl TimeSourceBuilder {
56
56
_use_clock_thread : self . use_clock_thread ,
57
57
_ros_time_active : Arc :: new ( Mutex :: new ( false ) ) ,
58
58
_clock_subscription : None ,
59
+ _last_time_msg : Arc :: new ( Mutex :: new ( None ) ) ,
59
60
} ;
60
61
source. attach_clock ( self . clock ) ;
61
62
source. attach_node ( self . node ) ;
@@ -76,13 +77,15 @@ impl fmt::Display for ClockMismatchError {
76
77
}
77
78
78
79
impl TimeSource {
79
- pub fn attach_clock ( & mut self , clock : Arc < Mutex < Clock > > ) -> Result < ( ) , ClockMismatchError > {
80
- {
81
- let clock = clock. lock ( ) . unwrap ( ) ;
82
- let clock_type = clock. clock_type ( ) ;
83
- if !matches ! ( clock_type, ClockType :: RosTime ) && * self . _ros_time_active . lock ( ) . unwrap ( ) {
84
- return Err ( ClockMismatchError ( clock_type) ) ;
85
- }
80
+ pub fn attach_clock ( & self , clock : Arc < Mutex < Clock > > ) -> Result < ( ) , ClockMismatchError > {
81
+ let clock_type = clock. clone ( ) . lock ( ) . unwrap ( ) . clock_type ( ) ;
82
+ if !matches ! ( clock_type, ClockType :: RosTime ) && * self . _ros_time_active . lock ( ) . unwrap ( ) {
83
+ return Err ( ClockMismatchError ( clock_type) ) ;
84
+ }
85
+ if let Some ( last_msg) = self . _last_time_msg . lock ( ) . unwrap ( ) . clone ( ) {
86
+ let nanoseconds: i64 =
87
+ ( last_msg. clock . sec as i64 * 1_000_000_000 ) + last_msg. clock . nanosec as i64 ;
88
+ Self :: update_clock ( & clock, nanoseconds) ;
86
89
}
87
90
// TODO(luca) this would allow duplicates to be stored in the vector but it seems other
88
91
// client libraries do the same, should we check and no-op if the value exists already?
@@ -91,7 +94,7 @@ impl TimeSource {
91
94
}
92
95
93
96
// TODO(luca) should we return a result to denote whether the clock was removed?
94
- pub fn detach_clock ( & mut self , clock : Arc < Mutex < Clock > > ) {
97
+ pub fn detach_clock ( & self , clock : Arc < Mutex < Clock > > ) {
95
98
self . _clocks
96
99
. lock ( )
97
100
. unwrap ( )
@@ -114,7 +117,8 @@ impl TimeSource {
114
117
self . _clock_subscription = Some ( self . create_clock_sub ( ) ?) ;
115
118
self . set_ros_time ( true ) ;
116
119
} else {
117
- // TODO(luca) cleanup subscription, clear set_ros_time
120
+ self . _clock_subscription = None ;
121
+ self . set_ros_time ( false ) ;
118
122
}
119
123
}
120
124
// TODO(luca) more graceful error handling
@@ -131,29 +135,35 @@ impl TimeSource {
131
135
}
132
136
}
133
137
138
+ fn update_clock ( clock : & Arc < Mutex < Clock > > , nanoseconds : i64 ) {
139
+ let clock = clock. lock ( ) . unwrap ( ) . rcl_clock ( ) ;
140
+ let mut clock = clock. lock ( ) . unwrap ( ) ;
141
+ // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
142
+ // by the mutex
143
+ unsafe {
144
+ rcl_set_ros_time_override ( & mut * clock, nanoseconds) ;
145
+ }
146
+ }
147
+
134
148
fn update_all_clocks ( clocks : & Arc < Mutex < Vec < Arc < Mutex < Clock > > > > > , nanoseconds : i64 ) {
135
149
let clocks = clocks. lock ( ) . unwrap ( ) ;
136
150
for clock in clocks. iter ( ) {
137
- let clock = clock. lock ( ) . unwrap ( ) . rcl_clock ( ) ;
138
- let mut clock = clock. lock ( ) . unwrap ( ) ;
139
- // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed
140
- // by the mutex
141
- unsafe {
142
- rcl_set_ros_time_override ( & mut * clock, nanoseconds) ;
143
- }
151
+ Self :: update_clock ( clock, nanoseconds) ;
144
152
}
145
153
}
146
154
147
155
fn create_clock_sub ( & self ) -> Result < Arc < Subscription < ClockMsg > > , RclrsError > {
148
156
let ros_time_active = self . _ros_time_active . clone ( ) ;
149
157
let clocks = self . _clocks . clone ( ) ;
158
+ let last_time_msg = self . _last_time_msg . clone ( ) ;
150
159
self . _node . create_subscription :: < ClockMsg , _ > (
151
160
"/clock" ,
152
161
self . _clock_qos ,
153
162
move |msg : ClockMsg | {
154
163
if * ros_time_active. lock ( ) . unwrap ( ) {
155
164
let nanoseconds: i64 =
156
165
( msg. clock . sec as i64 * 1_000_000_000 ) + msg. clock . nanosec as i64 ;
166
+ * last_time_msg. lock ( ) . unwrap ( ) = Some ( msg) ;
157
167
Self :: update_all_clocks ( & clocks, nanoseconds) ;
158
168
}
159
169
} ,
0 commit comments