Time is of the Essence (HULKs#990)

* Communication Rewrite * Ball types cleanup * Make behavior simulator wörk again * Improve error messages * improve logging and control flow * Restore fanta * Migrate LookAt Panel * revert some unused stuff in buffered_watch * fix rebase * disable recording for vision * draw lines blue, as they are blue in the image * draw ball filter hypotheses * allow to disable communication * port vision tuner * connect on address selection * fix types in kick decision layer * bump twix version * char instead of string * Port enum plot panel to new communication * fallback to ::1337 if framework.json disabled communication * parse nao address from command line arguments * remove todo * extract type * allow unused senders * Prevent behavior simulator panel from crashing * Set minimum visible duration to 10ms * remove unused repository_parameters.rs * cargo fmt * do not run this doctest * re-request already subscribed values once --------- Co-authored-by: Konrad Nölle <[email protected]> Co-authored-by: Narcha <[email protected]>
pejotejo · Jul 15, 2024 · e658b01 · e658b01
1 parent 50263cc
commit e658b01
Show file tree

Hide file tree

Showing 137 changed files with 5,908 additions and 9,736 deletions.
diff --git a/Cargo.lock b/Cargo.lock
diff --git a/crates/ball_filter/src/ball_position.rs b/crates/ball_filter/src/ball_position.rs
diff --git a/crates/ball_filter/src/hypothesis.rs b/crates/ball_filter/src/hypothesis.rs
@@ -10,9 +10,9 @@ use serde::{Deserialize, Serialize};
 use coordinate_systems::Ground;
 use linear_algebra::{vector, IntoFramed, Isometry2, Vector2};
 
-use types::multivariate_normal_distribution::MultivariateNormalDistribution;
-
-use crate::ball_position::BallPosition;
+use types::{
+    ball_position::BallPosition, multivariate_normal_distribution::MultivariateNormalDistribution,
+};
 
 pub mod moving;
 pub mod resting;

diff --git a/crates/ball_filter/src/lib.rs b/crates/ball_filter/src/lib.rs
@@ -6,10 +6,8 @@ use nalgebra::{Matrix2, Matrix4};
 use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 
-mod ball_position;
 mod hypothesis;
 
-pub use ball_position::BallPosition;
 pub use hypothesis::BallHypothesis;
 use types::multivariate_normal_distribution::MultivariateNormalDistribution;
 

diff --git a/crates/buffered_watch/src/receiver.rs b/crates/buffered_watch/src/receiver.rs
@@ -56,7 +56,7 @@ impl<T> Receiver<T> {
     }
 }
 
-fn lock_a_readable_buffer(states: &mut [State]) -> usize {
+pub(crate) fn lock_a_readable_buffer(states: &mut [State]) -> usize {
     let index = find_newest_readable_buffer(states);
 
     match states[index] {
@@ -115,9 +115,9 @@ impl<T> Drop for Receiver<T> {
 
 /// RAII guard for reading from a buffer
 pub struct ReceiverGuard<'lock, T> {
-    shared: RwLockReadGuard<'lock, Shared<T>>,
-    buffer_index: usize,
-    buffer: &'lock T,
+    pub(crate) shared: RwLockReadGuard<'lock, Shared<T>>,
+    pub(crate) buffer_index: usize,
+    pub(crate) buffer: &'lock T,
 }
 
 impl<T> Deref for ReceiverGuard<'_, T> {

diff --git a/crates/buffered_watch/src/sender.rs b/crates/buffered_watch/src/sender.rs
@@ -6,7 +6,9 @@ use std::{
 use parking_lot::{RwLock, RwLockReadGuard};
 use tokio::sync::watch;
 
-use crate::{find_oldest_free_buffer, Shared, State};
+use crate::{
+    find_oldest_free_buffer, receiver::lock_a_readable_buffer, ReceiverGuard, Shared, State,
+};
 
 /// Sends values to the associated Receivers
 pub struct Sender<T> {
@@ -18,6 +20,23 @@ unsafe impl<T> Sync for Sender<T> where T: Sync {}
 unsafe impl<T> Send for Sender<T> where T: Send + Sync {}
 
 impl<T> Sender<T> {
+    /// Borrows the latest value
+    pub fn borrow(&mut self) -> ReceiverGuard<T> {
+        let shared = self.shared.read();
+        let index = {
+            let states = &mut *shared.states.lock();
+            lock_a_readable_buffer(states)
+        };
+        // Safety: access is managed by the `shared.states`, we are allowed to dereference
+        let buffer = unsafe { &*shared.buffers[index].get() };
+
+        ReceiverGuard {
+            shared,
+            buffer_index: index,
+            buffer,
+        }
+    }
+
     /// Borrows a buffer to write to
     pub fn borrow_mut(&mut self) -> SenderGuard<T> {
         let shared = self.shared.read();

diff --git a/crates/code_generation/src/cyclers.rs b/crates/code_generation/src/cyclers.rs
@@ -127,9 +127,9 @@ fn generate_struct(cycler: &Cycler, cyclers: &Cyclers, mode: CyclerMode) -> Toke
         pub(crate) struct Cycler<HardwareInterface>  {
             instance: CyclerInstance,
             hardware_interface: std::sync::Arc<HardwareInterface>,
-            own_sender: buffered_watch::Sender<Database>,
+            own_sender: buffered_watch::Sender<(std::time::SystemTime, Database)>,
             own_subscribed_outputs_receiver: buffered_watch::Receiver<std::collections::HashSet<String>>,
-            parameters_receiver: buffered_watch::Receiver<crate::structs::Parameters>,
+            parameters_receiver: buffered_watch::Receiver<(std::time::SystemTime, crate::structs::Parameters)>,
             pub cycler_state: crate::structs::#module_name::CyclerState,
             #realtime_inputs
             #input_output_fields
@@ -165,7 +165,7 @@ fn generate_receiver_fields(cyclers: &Cyclers) -> TokenStream {
             let cycler_module_name = format_ident!("{}", cycler.name.to_case(Case::Snake));
 
             quote! {
-                #field_name: buffered_watch::Receiver<crate::cyclers::#cycler_module_name::Database>,
+                #field_name: buffered_watch::Receiver<(std::time::SystemTime, crate::cyclers::#cycler_module_name::Database)>,
             }
         })
         .collect()
@@ -251,14 +251,14 @@ fn generate_new_method(cycler: &Cycler, cyclers: &Cyclers, mode: CyclerMode) ->
         pub(crate) fn new(
             instance: CyclerInstance,
             hardware_interface: std::sync::Arc<HardwareInterface>,
-            own_sender: buffered_watch::Sender<Database>,
+            own_sender: buffered_watch::Sender<(std::time::SystemTime, Database)>,
             own_subscribed_outputs_receiver: buffered_watch::Receiver<std::collections::HashSet<String>>,
-            mut parameters_receiver: buffered_watch::Receiver<crate::structs::Parameters>,
+            mut parameters_receiver: buffered_watch::Receiver<(std::time::SystemTime, crate::structs::Parameters)>,
             #input_output_fields
             #recording_parameter_fields
         ) -> color_eyre::Result<Self> {
             let parameters_guard = parameters_receiver.borrow_and_mark_as_seen();
-            let parameters = &* parameters_guard;
+            let (_, parameters) = &* parameters_guard;
             let mut cycler_state = crate::structs::#cycler_module_name::CyclerState::default();
             #node_initializers
             drop(parameters_guard);
@@ -468,8 +468,11 @@ fn generate_cycle_method(cycler: &Cycler, cyclers: &Cyclers, mode: CyclerMode) -
         CyclerMode::Run => quote! {
             let now = <HardwareInterface as hardware::TimeInterface>::get_now(&*self.hardware_interface);
             let recording_timestamp = std::time::SystemTime::now();
+            *own_database_timestamp = recording_timestamp;
+        },
+        CyclerMode::Replay => quote! {
+            *own_database_timestamp = now;
         },
-        CyclerMode::Replay => Default::default(),
     };
     let post_setup = match cycler.kind {
         CyclerKind::Perception => quote! {
@@ -494,7 +497,7 @@ fn generate_cycle_method(cycler: &Cycler, cyclers: &Cyclers, mode: CyclerMode) -
                 let receiver = format_ident!("{}_receiver", instance.to_case(Case::Snake));
                 let database = format_ident!("{}_database", instance.to_case(Case::Snake));
                 quote! {
-                    let #database = self.#receiver.borrow_and_mark_as_seen();
+                    let (_, #database) = &*self.#receiver.borrow_and_mark_as_seen();
                 }
             })
             .collect(),
@@ -557,21 +560,23 @@ fn generate_cycle_method(cycler: &Cycler, cyclers: &Cyclers, mode: CyclerMode) -
             let instance_name = format!("{instance:?}");
             let itt_domain = ittapi::Domain::new(&instance_name);
 
-            let own_database = &mut *self.own_sender.borrow_mut();
+            let (own_database_timestamp, own_database) = &mut *self.own_sender.borrow_mut();
 
             #pre_setup
 
             {
                 let own_subscribed_outputs = self.own_subscribed_outputs_receiver.borrow_and_mark_as_seen();
-                let parameters = self.parameters_receiver.borrow_and_mark_as_seen();
+                let parameters_guard = self.parameters_receiver.borrow_and_mark_as_seen();
+                let (_, parameters) = &* parameters_guard;
                 #(#setup_node_executions)*
             }
 
             #post_setup
 
             {
                 let own_subscribed_outputs = self.own_subscribed_outputs_receiver.borrow_and_mark_as_seen();
-                let parameters = self.parameters_receiver.borrow_and_mark_as_seen();
+                let parameters_guard = self.parameters_receiver.borrow_and_mark_as_seen();
+                let (_, parameters) = &* parameters_guard;
                 #borrow_receivers
                 #cross_inputs
                 #(#cycle_node_executions)*