Skip to content

Commit 7676a41

Browse files
Minor cleanups, add debug code
Signed-off-by: Luca Della Vedova <[email protected]>
1 parent 5de661a commit 7676a41

File tree

3 files changed

+31
-37
lines changed

3 files changed

+31
-37
lines changed

examples/minimal_pub_sub/src/minimal_subscriber.rs

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ fn main() -> Result<(), Error> {
66
let context = rclrs::Context::new(env::args())?;
77

88
let node = rclrs::create_node(&context, "minimal_subscriber")?;
9+
let inner_node = node.clone();
910

1011
let mut num_messages: usize = 0;
1112

@@ -16,6 +17,8 @@ fn main() -> Result<(), Error> {
1617
num_messages += 1;
1718
println!("I heard: '{}'", msg.data);
1819
println!("(Got {} messages so far)", num_messages);
20+
let now = inner_node.get_clock().lock().unwrap().now();
21+
dbg!(now);
1922
},
2023
)?;
2124

rclrs/src/time.rs

Lines changed: 1 addition & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
use crate::clock::ClockType;
22
use crate::rcl_bindings::*;
33

4+
// TODO(luca) this is currently unused, maybe remove?
45
#[derive(Debug)]
56
pub struct Time {
67
pub nsec: i64,
@@ -15,23 +16,3 @@ impl From<Time> for rcl_time_point_t {
1516
}
1617
}
1718
}
18-
19-
/*
20-
impl Default for Time {
21-
fn default() -> Self {
22-
Self {
23-
nsec: 0,
24-
clock_type: ClockType::SystemTime,
25-
}
26-
}
27-
}
28-
29-
impl From<crate::vendor::builtin_interfaces::msg::Time> for Time {
30-
fn from(time_msg: crate::vendor::builtin_interfaces::msg::Time) -> Self {
31-
Self {
32-
nsec: (time_msg.sec as i64 * 1_000_000_000) + time_msg.nanosec as i64,
33-
clock_type: ClockType::RosTime,
34-
}
35-
}
36-
}
37-
*/

rclrs/src/time_source.rs

Lines changed: 27 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ pub struct TimeSource {
1818
// TODO(luca) Update this with parameter callbacks for use_sim_time
1919
_ros_time_active: Arc<Mutex<bool>>,
2020
_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>>>,
2222
}
2323

2424
pub struct TimeSourceBuilder {
@@ -56,6 +56,7 @@ impl TimeSourceBuilder {
5656
_use_clock_thread: self.use_clock_thread,
5757
_ros_time_active: Arc::new(Mutex::new(false)),
5858
_clock_subscription: None,
59+
_last_time_msg: Arc::new(Mutex::new(None)),
5960
};
6061
source.attach_clock(self.clock);
6162
source.attach_node(self.node);
@@ -76,13 +77,15 @@ impl fmt::Display for ClockMismatchError {
7677
}
7778

7879
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);
8689
}
8790
// TODO(luca) this would allow duplicates to be stored in the vector but it seems other
8891
// client libraries do the same, should we check and no-op if the value exists already?
@@ -91,7 +94,7 @@ impl TimeSource {
9194
}
9295

9396
// 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>>) {
9598
self._clocks
9699
.lock()
97100
.unwrap()
@@ -114,7 +117,8 @@ impl TimeSource {
114117
self._clock_subscription = Some(self.create_clock_sub()?);
115118
self.set_ros_time(true);
116119
} else {
117-
// TODO(luca) cleanup subscription, clear set_ros_time
120+
self._clock_subscription = None;
121+
self.set_ros_time(false);
118122
}
119123
}
120124
// TODO(luca) more graceful error handling
@@ -131,29 +135,35 @@ impl TimeSource {
131135
}
132136
}
133137

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+
134148
fn update_all_clocks(clocks: &Arc<Mutex<Vec<Arc<Mutex<Clock>>>>>, nanoseconds: i64) {
135149
let clocks = clocks.lock().unwrap();
136150
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);
144152
}
145153
}
146154

147155
fn create_clock_sub(&self) -> Result<Arc<Subscription<ClockMsg>>, RclrsError> {
148156
let ros_time_active = self._ros_time_active.clone();
149157
let clocks = self._clocks.clone();
158+
let last_time_msg = self._last_time_msg.clone();
150159
self._node.create_subscription::<ClockMsg, _>(
151160
"/clock",
152161
self._clock_qos,
153162
move |msg: ClockMsg| {
154163
if *ros_time_active.lock().unwrap() {
155164
let nanoseconds: i64 =
156165
(msg.clock.sec as i64 * 1_000_000_000) + msg.clock.nanosec as i64;
166+
*last_time_msg.lock().unwrap() = Some(msg);
157167
Self::update_all_clocks(&clocks, nanoseconds);
158168
}
159169
},

0 commit comments

Comments
 (0)