WIP Add minimal TimeSource implementation

Signed-off-by: Luca Della Vedova <[email protected]>

Author: luca-della-vedova

rclrs/Cargo.toml

@@ -31,5 +31,8 @@ tempfile = "3.3.0"
 # Needed for FFI
 bindgen = "0.66.1"
 
+[dependencies.rosgraph_msgs]
+version = "*"
+
 [features]
 dyn_msg = ["ament_rs", "libloading"]

rclrs/src/lib.rs

@@ -7,6 +7,7 @@
 
 mod arguments;
 mod client;
+mod clock;
 mod context;
 mod error;
 mod node;
@@ -15,6 +16,8 @@ mod publisher;
 mod qos;
 mod service;
 mod subscription;
+mod time;
+mod time_source;
 mod vendor;
 mod wait;
 
@@ -23,11 +26,12 @@ mod rcl_bindings;
 #[cfg(feature = "dyn_msg")]
 pub mod dynamic_message;
 
-use std::sync::Arc;
+use std::sync::{Arc, Mutex};
 use std::time::Duration;
 
 pub use arguments::*;
 pub use client::*;
+pub use clock::*;
 pub use context::*;
 pub use error::*;
 pub use node::*;
@@ -38,6 +42,8 @@ use rcl_bindings::rcl_context_is_valid;
 pub use rcl_bindings::rmw_request_id_t;
 pub use service::*;
 pub use subscription::*;
+pub use time::*;
+pub use time_source::*;
 pub use wait::*;
 
 /// Polls the node for new messages and executes the corresponding callbacks.
@@ -109,7 +115,12 @@ pub fn spin(node: Arc<Node>) -> Result<(), RclrsError> {
 /// # Ok::<(), RclrsError>(())
 /// ```
 pub fn create_node(context: &Context, node_name: &str) -> Result<Arc<Node>, RclrsError> {
-    Ok(Arc::new(Node::builder(context, node_name).build()?))
+    println!("Creating node");
+    let mut node = Arc::new(Node::builder(context, node_name).build()?);
+    *node._time_source.lock().unwrap() =
+        Some(TimeSourceBuilder::new(node.clone(), node.get_clock()).build());
+    Ok(node)
+    //Ok(Arc::new(Node::builder(context, node_name).build()?))
 }
 
 /// Creates a [`NodeBuilder`][1].

rclrs/src/node.rs

@@ -13,9 +13,9 @@ pub use self::builder::*;
 pub use self::graph::*;
 use crate::rcl_bindings::*;
 use crate::{
-    Client, ClientBase, Context, GuardCondition, ParameterOverrideMap, Publisher, QoSProfile,
-    RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback,
-    ToResult,
+    Client, ClientBase, Clock, Context, GuardCondition, ParameterOverrideMap, ParameterValue,
+    Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase,
+    SubscriptionCallback, TimeSource, TimeSourceBuilder, ToResult,
 };
 
 impl Drop for rcl_node_t {
@@ -71,6 +71,9 @@ pub struct Node {
     pub(crate) guard_conditions_mtx: Mutex<Vec<Weak<GuardCondition>>>,
     pub(crate) services_mtx: Mutex<Vec<Weak<dyn ServiceBase>>>,
     pub(crate) subscriptions_mtx: Mutex<Vec<Weak<dyn SubscriptionBase>>>,
+    _clock: Arc<Mutex<Clock>>,
+    // TODO(luca) set to private
+    pub _time_source: Arc<Mutex<Option<TimeSource>>>,
     _parameter_map: ParameterOverrideMap,
 }
 
@@ -96,7 +99,11 @@ impl Node {
     /// See [`NodeBuilder::new()`] for documentation.
     #[allow(clippy::new_ret_no_self)]
     pub fn new(context: &Context, node_name: &str) -> Result<Node, RclrsError> {
-        Self::builder(context, node_name).build()
+        Self::builder(context, node_name).build().into()
+    }
+
+    pub fn get_clock(&self) -> Arc<Mutex<Clock>> {
+        self._clock.clone()
     }
 
     /// Returns the name of the node.
@@ -355,6 +362,12 @@ impl Node {
         domain_id
     }
 
+    // TODO(luca) There should really be parameter callbacks, this is only for testing
+    // temporarily
+    pub fn get_parameter(&self, name: &str) -> Option<ParameterValue> {
+        self._parameter_map.get(name).cloned()
+    }
+
     /// Creates a [`NodeBuilder`][1] with the given name.
     ///
     /// Convenience function equivalent to [`NodeBuilder::new()`][2].

rclrs/src/node/builder.rs

@@ -3,8 +3,8 @@ use std::sync::{Arc, Mutex};
 
 use crate::rcl_bindings::*;
 use crate::{
-    node::call_string_getter_with_handle, resolve_parameter_overrides, Context, Node, RclrsError,
-    ToResult,
+    node::call_string_getter_with_handle, resolve_parameter_overrides, Clock, ClockType, Context,
+    Node, RclrsError, TimeSource, TimeSourceBuilder, ToResult,
 };
 
 /// A builder for creating a [`Node`][1].
@@ -17,6 +17,7 @@ use crate::{
 /// - `use_global_arguments: true`
 /// - `arguments: []`
 /// - `enable_rosout: true`
+/// - `clock_type: ClockType::RosTime`
 ///
 /// # Example
 /// ```
@@ -46,6 +47,7 @@ pub struct NodeBuilder {
     use_global_arguments: bool,
     arguments: Vec<String>,
     enable_rosout: bool,
+    clock_type: ClockType,
 }
 
 impl NodeBuilder {
@@ -92,6 +94,7 @@ impl NodeBuilder {
             use_global_arguments: true,
             arguments: vec![],
             enable_rosout: true,
+            clock_type: ClockType::RosTime,
         }
     }
 
@@ -262,6 +265,8 @@ impl NodeBuilder {
             .ok()?;
         };
 
+        let clock = Clock::new(self.clock_type)?;
+
         let _parameter_map = unsafe {
             let fqn = call_string_getter_with_handle(&rcl_node, rcl_node_get_fully_qualified_name);
             resolve_parameter_overrides(
@@ -271,16 +276,24 @@ impl NodeBuilder {
             )?
         };
         let rcl_node_mtx = Arc::new(Mutex::new(rcl_node));
-
-        Ok(Node {
+        let mut node = Node {
             rcl_node_mtx,
             rcl_context_mtx: self.context.clone(),
             clients_mtx: Mutex::new(vec![]),
             guard_conditions_mtx: Mutex::new(vec![]),
             services_mtx: Mutex::new(vec![]),
             subscriptions_mtx: Mutex::new(vec![]),
+            _clock: Arc::new(Mutex::new(clock)),
+            _time_source: Arc::new(Mutex::new(None)),
             _parameter_map,
-        })
+        };
+        /*
+        let node_mtx = Arc::new(node);
+        node._time_source = Some(Arc::new(Mutex::new(TimeSourceBuilder::new(node_mtx).build())));
+        */
+        //node._time_source = Some(Arc::new(Mutex::new(TimeSourceBuilder::new(node_mtx).build())));
+
+        Ok(node)
     }
 
     /// Creates a rcl_node_options_t struct from this builder.

rclrs/src/qos.rs

@@ -296,6 +296,21 @@ pub const QOS_PROFILE_SENSOR_DATA: QoSProfile = QoSProfile {
     avoid_ros_namespace_conventions: false,
 };
 
+/// Equivalent to `ClockQos` from the [`rclcpp` package][1].
+/// Same as sensor data but with a history depth of 1
+///
+/// [1]: https://github.com/ros2/rclcpp/blob/rolling/rclcpp/include/rclcpp/qos.hpp
+pub const QOS_PROFILE_CLOCK: QoSProfile = QoSProfile {
+    history: QoSHistoryPolicy::KeepLast { depth: 1 },
+    reliability: QoSReliabilityPolicy::BestEffort,
+    durability: QoSDurabilityPolicy::Volatile,
+    deadline: QoSDuration::SystemDefault,
+    lifespan: QoSDuration::SystemDefault,
+    liveliness: QoSLivelinessPolicy::SystemDefault,
+    liveliness_lease_duration: QoSDuration::SystemDefault,
+    avoid_ros_namespace_conventions: false,
+};
+
 /// Equivalent to `rmw_qos_profile_parameters` from the [`rmw` package][1].
 ///
 /// [1]: https://github.com/ros2/rmw/blob/master/rmw/include/rmw/qos_profiles.h