PathPlanner

MathLib/build.gradle

@@ -1,13 +1,15 @@
 plugins {
     id 'java-library'
-    id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3"
+//    id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3"
 }
 
 import org.gradle.api.tasks.testing.logging.TestExceptionFormat
 import org.gradle.api.tasks.testing.logging.TestLogEvent
 
-sourceCompatibility = JavaVersion.VERSION_17
-targetCompatibility = JavaVersion.VERSION_17
+java {
+    sourceCompatibility = JavaVersion.VERSION_17
+    targetCompatibility = JavaVersion.VERSION_17
+}
 
 group 'com.swrobotics'
 version '2024'

Messenger/messenger-client/src/lib.rs

@@ -3,7 +3,7 @@ use std::time::Duration;
 use bytes::{Buf, BufMut, Bytes, BytesMut};
 use tokio::{
     io::AsyncWriteExt,
-    net::TcpStream,
+    net::{tcp::OwnedWriteHalf, TcpStream},
     sync::mpsc::{self, UnboundedReceiver, UnboundedSender},
     task::JoinHandle,
 };
@@ -12,8 +12,8 @@ use tokio_util::codec::{Decoder, FramedRead};
 
 #[derive(Debug)]
 pub struct Message {
-    name: String,
-    data: Bytes,
+    pub name: String,
+    pub data: Bytes,
 }
 
 struct MessageDecoder;
@@ -69,6 +69,7 @@ impl Decoder for MessageDecoder {
 }
 
 enum ToIoCmd {
+    Listen(String),
     SendMessage(Message),
     Disconnect,
 }
@@ -86,11 +87,26 @@ fn pack_message(m: Message) -> BytesMut {
     dst
 }
 
+async fn send_listen_message(
+    write_half: &mut OwnedWriteHalf,
+    name: &str,
+) -> Result<(), std::io::Error> {
+    let mut data = BytesMut::with_capacity(name.len() + 2);
+    pack_str(&name, &mut data);
+    write_half
+        .write_all(&pack_message(Message {
+            name: "_Listen".to_string(),
+            data: data.into(),
+        }))
+        .await
+}
+
 async fn message_io(
     addr: &str,
     name: &str,
     in_send: &mpsc::UnboundedSender<Message>,
     out_recv: &mut mpsc::UnboundedReceiver<ToIoCmd>,
+    listening: &mut Vec<String>,
 ) -> Result<(), std::io::Error> {
     let stream = TcpStream::connect(addr).await?;
     let (read_half, mut write_half) = stream.into_split();
@@ -100,6 +116,12 @@ async fn message_io(
     pack_str(name, &mut name_buf);
     write_half.write_all(&name_buf).await?;
 
+    println!("Messenger connected");
+
+    for name in listening {
+        send_listen_message(&mut write_half, &name).await?;
+    }
+
     let mut read = FramedRead::new(read_half, MessageDecoder);
 
     let mut heartbeat_interval = tokio::time::interval(Duration::from_secs(1));
@@ -111,14 +133,15 @@ async fn message_io(
     loop {
         tokio::select! {
             _ = heartbeat_interval.tick() => {
-                println!("Writing heartbeat");
                 write_half.write_all(&packed_heartbeat).await?;
             }
 
             result = out_recv.recv() => {match result {
                 Some(cmd) => match cmd {
+                    ToIoCmd::Listen(name) => {
+                        send_listen_message(&mut write_half, &name).await?;
+                    }
                     ToIoCmd::SendMessage(msg) => {
-                        println!("Writing message: {:?}", msg);
                         write_half.write_all(&pack_message(msg)).await?;
                     }
                     ToIoCmd::Disconnect => break
@@ -128,17 +151,18 @@ async fn message_io(
 
             result = read.next() => match result {
                 Some(res) => {
-                    println!("Read: {:?}", res);
-                    if let Err(_) = in_send.send(res?) {
-                        break
+                    let msg = res?;
+                    if msg.name != "_Heartbeat" {
+                        if let Err(_) = in_send.send(msg) {
+                            break
+                        }
                     }
                 }
                 None => return Err(std::io::Error::from(std::io::ErrorKind::BrokenPipe))
             }
         }
     }
 
-    println!("Disconnecting");
     write_half
         .write_all(&pack_message(Message {
             name: "_Disconnect".to_string(),
@@ -156,15 +180,17 @@ pub struct MessengerClient {
 }
 
 impl MessengerClient {
-    pub async fn new(addr: String, name: String) -> Self {
+    pub fn new(addr: String, name: String) -> Self {
         let (in_send, in_recv) = mpsc::unbounded_channel();
         let (out_send, mut out_recv) = mpsc::unbounded_channel();
         let io_join = tokio::spawn(async move {
+            let mut listening = vec![];
+
             loop {
-                match message_io(&addr, &name, &in_send, &mut out_recv).await {
+                match message_io(&addr, &name, &in_send, &mut out_recv, &mut listening).await {
                     // Ok indicates client disconnected cleanly
                     Ok(_) => break,
-                    Err(e) => println!("Connection failed: {}", e),
+                    Err(e) => println!("Messenger connection failed: {}", e),
                 }
 
                 tokio::time::sleep(Duration::from_secs(1)).await;
@@ -178,14 +204,51 @@ impl MessengerClient {
         }
     }
 
+    pub fn listen(&mut self, name: &str) {
+        let _ = self.out_send.send(ToIoCmd::Listen(name.to_string()));
+    }
+
+    pub fn listen_multiple(&mut self, names: Vec<&str>) {
+        for name in names {
+            self.listen(name);
+        }
+    }
+
     pub fn send_message(&mut self, msg: Message) {
         let _ = self.out_send.send(ToIoCmd::SendMessage(msg));
     }
 
+    pub fn send_empty(&mut self, name: &str) {
+        self.send_message(Message {
+            name: name.to_string(),
+            data: Bytes::new(),
+        });
+    }
+
     pub fn poll_recv_message(&mut self) -> Option<Message> {
         self.in_recv.try_recv().ok()
     }
 
+    /// Gets all available incoming messages immediately if any are available,
+    /// otherwise waits for one to arrive
+    pub async fn poll_or_await_messages(&mut self) -> Vec<Message> {
+        match self.poll_recv_message() {
+            Some(msg) => {
+                let mut messages = vec![msg];
+                loop {
+                    match self.poll_recv_message() {
+                        Some(m) => messages.push(m),
+                        None => return messages,
+                    }
+                }
+            }
+            None => match self.in_recv.recv().await {
+                Some(m) => vec![m],
+                None => vec![],
+            },
+        }
+    }
+
     pub async fn disconnect(self) {
         match self.out_send.send(ToIoCmd::Disconnect) {
             Ok(_) => {