From 910d6282b3ee7554e1bc0e1dcd80b82c8e09eaa8 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Sun, 14 Sep 2025 13:29:04 -0400
Subject: [PATCH 01/19] Add ros2 stuff to gitignore

---
 .gitignore | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/.gitignore b/.gitignore
index 9b144e1..66016ad 100644
--- a/.gitignore
+++ b/.gitignore
@@ -21,3 +21,7 @@ target
 # Contains mutation testing data
 **/mutants.out*/
 
+#Ros2 stuff
+install/
+build/
+log/

From c4bd70d67486cb85c70f3e2a421a52634b71d362 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Sun, 14 Sep 2025 14:32:33 -0400
Subject: [PATCH 02/19] Change hashmap to struct

---
 src/mission-planner/src/cmission.rs           | 16 ++++-----
 .../src/concurrent_mission_example.rs         | 12 ++++---
 src/mission-planner/src/main.rs               |  1 +
 src/mission-planner/src/mission.rs            | 36 +++++++++++++------
 src/mission-planner/src/mission_example.rs    | 13 +++----
 src/mission-planner/src/mission_scheduler.rs  |  8 ++---
 src/mission-planner/src/pymission.rs          | 17 ++++-----
 7 files changed, 62 insertions(+), 41 deletions(-)

diff --git a/src/mission-planner/src/cmission.rs b/src/mission-planner/src/cmission.rs
index 6575e3f..2c1c93b 100644
--- a/src/mission-planner/src/cmission.rs
+++ b/src/mission-planner/src/cmission.rs
@@ -1,4 +1,4 @@
-use crate::mission::{Mission, MissionHashMap, MissionResult, Task};
+use crate::mission::{Mission, MissionData, MissionResult, Task};
 use std::ffi::{c_char, CStr};
 
 #[repr(C)]
@@ -16,10 +16,10 @@ pub enum OptionFunction<T> {
     None
 }
 
-pub struct MissionMapPtr;
+pub struct MissionDataPtr;
 
 
-type CTaskFunc = unsafe extern "C" fn(data: *mut MissionMapPtr) -> CMissionResult;
+type CTaskFunc = unsafe extern "C" fn(data: *mut MissionDataPtr) -> CMissionResult;
 
 pub struct CTask {
     name: String,
@@ -27,15 +27,15 @@ pub struct CTask {
     repair_task_func: OptionFunction<CTaskFunc>,
 }
 
-fn run_with(func: &OptionFunction<CTaskFunc>, data: &MissionHashMap) -> MissionResult {
+fn run_with(func: &OptionFunction<CTaskFunc>, data: &MissionData) -> MissionResult {
     let OptionFunction::Some(func) = func else {
         return Err(false)
     };
     let data_ptr = Box::new(data);
-    let data_ptr = Box::into_raw(data_ptr) as *mut MissionMapPtr;
+    let data_ptr = Box::into_raw(data_ptr) as *mut MissionDataPtr;
     let res = unsafe { func(data_ptr) };
     //Deallocate pointer
-    let _ = unsafe { Box::from_raw(data_ptr as *mut MissionHashMap)};
+    let _ = unsafe { Box::from_raw(data_ptr as *mut MissionData)};
     match res {
         CMissionResult::Ok => Ok(()),
         CMissionResult::Err => Err(false),
@@ -44,10 +44,10 @@ fn run_with(func: &OptionFunction<CTaskFunc>, data: &MissionHashMap) -> MissionR
 }
 
 impl Task for CTask {
-    fn run(&self, data: &MissionHashMap) -> MissionResult {
+    fn run(&self, data: &MissionData) -> MissionResult {
         run_with(&self.task_func, data)
     }
-    fn repair_run(&self, data: &MissionHashMap) -> MissionResult {
+    fn repair_run(&self, data: &MissionData) -> MissionResult {
         run_with(&self.repair_task_func, data)
     }
     fn name(&self) -> &String {
diff --git a/src/mission-planner/src/concurrent_mission_example.rs b/src/mission-planner/src/concurrent_mission_example.rs
index fec3063..4673ded 100644
--- a/src/mission-planner/src/concurrent_mission_example.rs
+++ b/src/mission-planner/src/concurrent_mission_example.rs
@@ -1,9 +1,11 @@
-use crate::mission::{Mission, MissionResult, MissionHashMap, RustTask, Task};
+use std::sync::atomic::Ordering;
 
-fn conc_example(data: &MissionHashMap) -> MissionResult {
-    if data.contains_key("flag_request") {
-        println!("Wrote flag");
-        data.insert("flag".to_string(), "true".to_string());
+use crate::mission::{Mission, MissionResult, MissionData, RustTask, Task};
+
+fn conc_example(data: &MissionData) -> MissionResult {
+    if data.example_flag_request.load(Ordering::Relaxed) {
+        data.example_flag.store(true, Ordering::Relaxed);
+        println!("Wrote flag!");
     }
     Ok(())
 }
diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index 4197d12..affb07d 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -4,6 +4,7 @@ mod cmission;
 mod pymission;
 mod mission_example;
 mod concurrent_mission_example;
+mod ros_sub_mission_example;
 
 include!(concat!(env!("OUT_DIR"), "/bindings.rs"));
 
diff --git a/src/mission-planner/src/mission.rs b/src/mission-planner/src/mission.rs
index eb19c1c..06819f4 100644
--- a/src/mission-planner/src/mission.rs
+++ b/src/mission-planner/src/mission.rs
@@ -3,19 +3,35 @@ use dashmap::DashMap;
 pub type MissionHashMap = DashMap<String, String>;
 pub type MissionResult = Result<(), bool>;
 
+#[derive(Debug)]
+#[pyo3::pyclass]
+pub struct MissionData {
+    pub example_flag : AtomicBool,
+    pub example_flag_request : AtomicBool,
+}
+
+impl MissionData {
+    pub fn new() -> Self {
+        MissionData {
+            example_flag: AtomicBool::new(false),
+            example_flag_request: AtomicBool::new(false),
+        }
+    }
+}
+
 pub trait Task : Send + Sync {
-    fn run(&self, data: &MissionHashMap) -> MissionResult;
-    fn repair_run(&self, data: &MissionHashMap) -> MissionResult;
+    fn run(&self, data: &MissionData) -> MissionResult;
+    fn repair_run(&self, data: &MissionData) -> MissionResult;
     fn name(&self) -> &String;
 }
 
 pub struct RustTask {
     pub name: String,
-    func: Option<fn(&MissionHashMap) -> MissionResult>,
-    repair_func: Option<fn(&MissionHashMap) -> MissionResult>
+    func: Option<fn(&MissionData) -> MissionResult>,
+    repair_func: Option<fn(&MissionData) -> MissionResult>
 }
 
-fn run_with(func: Option<fn(&MissionHashMap) -> MissionResult>, data: &MissionHashMap) -> MissionResult {
+fn run_with(func: Option<fn(&MissionData) -> MissionResult>, data: &MissionData) -> MissionResult {
     let Some(func) = func else {
         return Err(false);
     };
@@ -23,8 +39,8 @@ fn run_with(func: Option<fn(&MissionHashMap) -> MissionResult>, data: &MissionHa
 }
 
 impl RustTask {
-    pub fn new(name: String, func: Option< fn(&MissionHashMap) -> MissionResult>,
-    repair_func: Option<fn(&MissionHashMap)-> MissionResult>) -> RustTask {
+    pub fn new(name: String, func: Option< fn(&MissionData) -> MissionResult>,
+    repair_func: Option<fn(&MissionData)-> MissionResult>) -> RustTask {
         RustTask {
             name,
             func,
@@ -34,10 +50,10 @@ impl RustTask {
 }
 
 impl Task for RustTask {
-    fn run(&self, data: &MissionHashMap) -> MissionResult {
+    fn run(&self, data: &MissionData) -> MissionResult {
         run_with(self.func, data)
     }
-    fn repair_run(&self, data: &MissionHashMap) -> MissionResult {
+    fn repair_run(&self, data: &MissionData) -> MissionResult {
         run_with(self.repair_func, data)
     }
     fn name(&self) -> &String {
@@ -51,7 +67,7 @@ pub struct Mission {
 }
 
 impl Mission {
-    pub fn run(&self, data: &DashMap<String, String>) -> MissionResult {
+    pub fn run(&self, data: &MissionData) -> MissionResult {
         if self.task_list.is_empty() {
             return Ok(())
         }
diff --git a/src/mission-planner/src/mission_example.rs b/src/mission-planner/src/mission_example.rs
index d7de448..7995981 100644
--- a/src/mission-planner/src/mission_example.rs
+++ b/src/mission-planner/src/mission_example.rs
@@ -1,16 +1,17 @@
 use std::thread::sleep;
-use crate::mission::{Mission, Task, MissionResult, MissionHashMap, RustTask};
+use crate::mission::{Mission, Task, MissionResult, MissionData, RustTask};
+use std::sync::atomic::Ordering;
 
-fn example(_data: &MissionHashMap) -> MissionResult {
-    println!("Hello world!");
+fn example(_data: &MissionData) -> MissionResult {
+    println!("Hello from Rust!");
     Err(false)
 }
 
-fn repair_example(data: &MissionHashMap) -> MissionResult {
+fn repair_example(data: &MissionData) -> MissionResult {
     println!("Requested flag");
-    data.insert("flag_request".to_string(), "true".to_string());
+    data.example_flag_request.store(true, Ordering::Relaxed);
 
-    while ! data.contains_key("flag") {
+    while ! data.example_flag.load(Ordering::Relaxed) {
         sleep(std::time::Duration::from_millis(100));
     }
     println!("Got flag!");
diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index ed9e0f2..f61f964 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -1,5 +1,5 @@
 use dashmap::DashMap;
-use crate::mission::{Mission, MissionHashMap};
+use crate::mission::{Mission, MissionData};
 use std::collections::VecDeque;
 use std::sync::{Arc, Mutex};
 use std::sync::atomic::{AtomicBool, Ordering};
@@ -9,7 +9,7 @@ pub type MissionVec = VecDeque<Mission>;
 struct MissionThreadData {
     mission_list: Arc<Mutex<MissionVec>>,
     conc_mission_list: Arc<Mutex<MissionVec>>,
-    mission_data: Arc<DashMap<String, String>>,
+    mission_data: Arc<MissionData>,
     run: AtomicBool,
     stop: AtomicBool,
     waiting: AtomicBool,
@@ -21,7 +21,7 @@ impl MissionThreadData {
         Self {
             mission_list: Arc::new(Mutex::new(VecDeque::new())),
             conc_mission_list: Arc::new(Mutex::new(VecDeque::new())),
-            mission_data: Arc::new(DashMap::new()),
+            mission_data: Arc::new(MissionData::new()),
             run: AtomicBool::new(false),
             stop: AtomicBool::new(false),
             waiting: AtomicBool::new(false),
@@ -95,7 +95,7 @@ impl MissionScheduler {
         self.scheduler_data.with_mission_list(func, true);
     }
 
-    pub fn get_data(&self) -> Arc<MissionHashMap> {
+    pub fn get_data(&self) -> Arc<MissionData> {
         self.scheduler_data.mission_data.clone()
     }
 
diff --git a/src/mission-planner/src/pymission.rs b/src/mission-planner/src/pymission.rs
index c425cb0..e88dd4a 100644
--- a/src/mission-planner/src/pymission.rs
+++ b/src/mission-planner/src/pymission.rs
@@ -1,8 +1,8 @@
 use std::ffi::CStr;
 
-use crate::mission::{Mission, Task, MissionHashMap, MissionResult};
+use crate::mission::{Mission, Task, MissionData, MissionResult};
 
-use pyo3::{ffi::c_str, prelude::*, types::IntoPyDict, PyResult, Python};
+use pyo3::{PyResult, Python, ffi::c_str, prelude::*, types::{IntoPyDict, PyDict}};
 
 pub struct PyTask {
     name: String,
@@ -25,12 +25,13 @@ impl PyTask {
         }
     }
 
-    fn run_with(&self, option: &str, data: &MissionHashMap) -> MissionResult {
+    fn run_with(&self, option: &str, data: &MissionData) -> MissionResult {
         let res = Python::with_gil(|py| -> PyResult<()> {
             let task_obj = self.pytask_obj.as_ref();
-            //TODO: This implies read-only access
-            let dict = data.clone().into_py_dict(py)?;
-            let args = (dict, );
+            //let dict = data.clone().into_py_dict(py)?;
+            //For now let's just pass a dummy data
+            let dict = PyDict::new(py);
+            let args = (dict,);
             task_obj.call_method1(py, option, args)?;
             Ok(())
         });
@@ -46,12 +47,12 @@ impl PyTask {
 }
 
 impl Task for PyTask {
-    fn run(&self, data: &MissionHashMap) -> MissionResult {
+    fn run(&self, data: &MissionData) -> MissionResult {
         //TODO: Handle errors from Python and not simply assume everything is fine
         self.run_with("run", data)
     }
 
-    fn repair_run(&self, data: &MissionHashMap) -> MissionResult {
+    fn repair_run(&self, data: &MissionData) -> MissionResult {
         self.run_with("repair_run", data)
     }
 

From 85c6ccf40e306fdcacf0bc4d126ff0ce273ef813 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Tue, 16 Sep 2025 16:25:38 -0400
Subject: [PATCH 03/19] Add ros capability

---
 Cargo.lock                                   | 602 +++++++++++++++++--
 src/mission-planner/Cargo.toml               |   3 +-
 src/mission-planner/bindings.h               |   4 +-
 src/mission-planner/src/cmission_example.c   |   4 +-
 src/mission-planner/src/main.rs              |  13 +-
 src/mission-planner/src/mission.rs           |   4 +-
 src/mission-planner/src/mission_scheduler.rs | 101 +++-
 7 files changed, 647 insertions(+), 84 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 4d4e8a8..248127e 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -67,6 +67,26 @@ version = "1.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8"
 
+[[package]]
+name = "bindgen"
+version = "0.71.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5f58bf3d7db68cfbac37cfc485a8d711e87e064c3d0fe0435b92f7a407f9d6b3"
+dependencies = [
+ "bitflags",
+ "cexpr",
+ "clang-sys",
+ "itertools 0.13.0",
+ "log",
+ "prettyplease",
+ "proc-macro2",
+ "quote",
+ "regex",
+ "rustc-hash",
+ "shlex",
+ "syn",
+]
+
 [[package]]
 name = "bindgen"
 version = "0.72.0"
@@ -76,7 +96,7 @@ dependencies = [
  "bitflags",
  "cexpr",
  "clang-sys",
- "itertools",
+ "itertools 0.13.0",
  "log",
  "prettyplease",
  "proc-macro2",
@@ -93,6 +113,21 @@ version = "2.9.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967"
 
+[[package]]
+name = "block-buffer"
+version = "0.10.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71"
+dependencies = [
+ "generic-array",
+]
+
+[[package]]
+name = "bumpalo"
+version = "3.19.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "46c5e41b57b8bba42a04676d81cb89e9ee8e859a1a66f80a5a72e1cb76b34d43"
+
 [[package]]
 name = "cbindgen"
 version = "0.29.0"
@@ -180,6 +215,34 @@ version = "1.0.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b05b61dc5112cbb17e4b6cd61790d9845d13888356391624cbe7e41efeac1e75"
 
+[[package]]
+name = "cpufeatures"
+version = "0.2.17"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "59ed5838eebb26a2bb2e58f6d5b5316989ae9d08bab10e0e6d103e656d1b0280"
+dependencies = [
+ "libc",
+]
+
+[[package]]
+name = "crossbeam-deque"
+version = "0.8.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51"
+dependencies = [
+ "crossbeam-epoch",
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-epoch"
+version = "0.9.18"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e"
+dependencies = [
+ "crossbeam-utils",
+]
+
 [[package]]
 name = "crossbeam-utils"
 version = "0.8.21"
@@ -187,17 +250,23 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28"
 
 [[package]]
-name = "dashmap"
-version = "6.1.0"
+name = "crypto-common"
+version = "0.1.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5041cc499144891f3790297212f32a74fb938e5136a14943f338ef9e0ae276cf"
+checksum = "1bfb12502f3fc46cca1bb51ac28df9d618d813cdc3d2f25b9fe775a34af26bb3"
 dependencies = [
- "cfg-if",
- "crossbeam-utils",
- "hashbrown 0.14.5",
- "lock_api",
- "once_cell",
- "parking_lot_core",
+ "generic-array",
+ "typenum",
+]
+
+[[package]]
+name = "digest"
+version = "0.10.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292"
+dependencies = [
+ "block-buffer",
+ "crypto-common",
 ]
 
 [[package]]
@@ -228,6 +297,112 @@ version = "2.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "37909eebbb50d72f9059c3b6d82c0463f2ff062c9e95845c43a6c9c0355411be"
 
+[[package]]
+name = "force-send-sync"
+version = "1.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "eb5682bd193aa441fe571a005561e0c0ada74f89ec2dc390483a6ff17c0665b9"
+
+[[package]]
+name = "futures"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "65bc07b1a8bc7c85c5f2e110c476c7389b4554ba72af57d8445ea63a576b0876"
+dependencies = [
+ "futures-channel",
+ "futures-core",
+ "futures-executor",
+ "futures-io",
+ "futures-sink",
+ "futures-task",
+ "futures-util",
+]
+
+[[package]]
+name = "futures-channel"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2dff15bf788c671c1934e366d07e30c1814a8ef514e1af724a602e8a2fbe1b10"
+dependencies = [
+ "futures-core",
+ "futures-sink",
+]
+
+[[package]]
+name = "futures-core"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e"
+
+[[package]]
+name = "futures-executor"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1e28d1d997f585e54aebc3f97d39e72338912123a67330d723fdbb564d646c9f"
+dependencies = [
+ "futures-core",
+ "futures-task",
+ "futures-util",
+ "num_cpus",
+]
+
+[[package]]
+name = "futures-io"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6"
+
+[[package]]
+name = "futures-macro"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "futures-sink"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7"
+
+[[package]]
+name = "futures-task"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988"
+
+[[package]]
+name = "futures-util"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81"
+dependencies = [
+ "futures-channel",
+ "futures-core",
+ "futures-io",
+ "futures-macro",
+ "futures-sink",
+ "futures-task",
+ "memchr",
+ "pin-project-lite",
+ "pin-utils",
+ "slab",
+]
+
+[[package]]
+name = "generic-array"
+version = "0.14.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a"
+dependencies = [
+ "typenum",
+ "version_check",
+]
+
 [[package]]
 name = "getrandom"
 version = "0.3.3"
@@ -246,12 +421,6 @@ version = "0.3.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2"
 
-[[package]]
-name = "hashbrown"
-version = "0.14.5"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1"
-
 [[package]]
 name = "hashbrown"
 version = "0.15.4"
@@ -264,6 +433,12 @@ version = "0.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea"
 
+[[package]]
+name = "hermit-abi"
+version = "0.5.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "fc0fef456e4baa96da950455cd02c081ca953b141298e41db3fc7e36b1da849c"
+
 [[package]]
 name = "indexmap"
 version = "2.10.0"
@@ -271,7 +446,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "fe4cd85333e22411419a0bcae1297d25e58c9443848b11dc6a86fefe8c78a661"
 dependencies = [
  "equivalent",
- "hashbrown 0.15.4",
+ "hashbrown",
 ]
 
 [[package]]
@@ -286,6 +461,15 @@ version = "1.70.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "7943c866cc5cd64cbc25b2e01621d07fa8eb2a1a23160ee81ce38704e97b8ecf"
 
+[[package]]
+name = "itertools"
+version = "0.10.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473"
+dependencies = [
+ "either",
+]
+
 [[package]]
 name = "itertools"
 version = "0.13.0"
@@ -301,6 +485,22 @@ version = "1.0.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "4a5f13b858c8d314ee3e8f639011f7ccefe71f97f96e50151fb991f267928e2c"
 
+[[package]]
+name = "js-sys"
+version = "0.3.78"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0c0b063578492ceec17683ef2f8c5e89121fbd0b172cbc280635ab7567db2738"
+dependencies = [
+ "once_cell",
+ "wasm-bindgen",
+]
+
+[[package]]
+name = "lazy_static"
+version = "1.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe"
+
 [[package]]
 name = "libc"
 version = "0.2.174"
@@ -323,16 +523,6 @@ version = "0.9.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "cd945864f07fe9f5371a27ad7b52a172b4b499999f1d97574c9fa68373937e12"
 
-[[package]]
-name = "lock_api"
-version = "0.4.13"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "96936507f153605bddfcda068dd804796c84324ed2510809e5b2a624c81da765"
-dependencies = [
- "autocfg",
- "scopeguard",
-]
-
 [[package]]
 name = "log"
 version = "0.4.27"
@@ -364,11 +554,12 @@ checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a"
 name = "mission_planner"
 version = "0.1.0"
 dependencies = [
- "bindgen",
+ "bindgen 0.72.0",
  "cbindgen",
  "cc",
- "dashmap",
+ "futures",
  "pyo3",
+ "r2r",
 ]
 
 [[package]]
@@ -381,6 +572,16 @@ dependencies = [
  "minimal-lexical",
 ]
 
+[[package]]
+name = "num_cpus"
+version = "1.17.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "91df4bbde75afed763b708b7eee1e8e7651e02d97f6d5dd763e89367e957b23b"
+dependencies = [
+ "hermit-abi",
+ "libc",
+]
+
 [[package]]
 name = "once_cell"
 version = "1.21.3"
@@ -394,18 +595,74 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a4895175b425cb1f87721b59f0f286c2092bd4af812243672510e1ac53e2e0ad"
 
 [[package]]
-name = "parking_lot_core"
-version = "0.9.11"
+name = "os_str_bytes"
+version = "6.6.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bc838d2a56b5b1a6c25f55575dfc605fabb63bb2365f6c2353ef9159aa69e4a5"
+checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1"
 dependencies = [
- "cfg-if",
- "libc",
- "redox_syscall",
- "smallvec",
- "windows-targets 0.52.6",
+ "memchr",
+]
+
+[[package]]
+name = "paste"
+version = "1.0.15"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
+
+[[package]]
+name = "phf"
+version = "0.11.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1fd6780a80ae0c52cc120a26a1a42c1ae51b247a253e4e06113d23d2c2edd078"
+dependencies = [
+ "phf_macros",
+ "phf_shared",
+]
+
+[[package]]
+name = "phf_generator"
+version = "0.11.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3c80231409c20246a13fddb31776fb942c38553c51e871f8cbd687a4cfb5843d"
+dependencies = [
+ "phf_shared",
+ "rand",
+]
+
+[[package]]
+name = "phf_macros"
+version = "0.11.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f84ac04429c13a7ff43785d75ad27569f2951ce0ffd30a3321230db2fc727216"
+dependencies = [
+ "phf_generator",
+ "phf_shared",
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "phf_shared"
+version = "0.11.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "67eabc2ef2a60eb7faa00097bd1ffdb5bd28e62bf39990626a582201b7a754e5"
+dependencies = [
+ "siphasher",
 ]
 
+[[package]]
+name = "pin-project-lite"
+version = "0.2.16"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b"
+
+[[package]]
+name = "pin-utils"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
+
 [[package]]
 name = "portable-atomic"
 version = "1.11.1"
@@ -509,12 +766,131 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "69cdb34c158ceb288df11e18b4bd39de994f6657d83847bdffdbd7f346754b0f"
 
 [[package]]
-name = "redox_syscall"
-version = "0.5.17"
+name = "r2r"
+version = "0.9.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5407465600fb0548f1442edf71dd20683c6ed326200ace4b1ef0763521bb3b77"
+checksum = "b36b61a6252e8e7a24919b9877dacf1dc9f0ac4c2d0eba91c68b35c1b2283ed2"
 dependencies = [
- "bitflags",
+ "force-send-sync",
+ "futures",
+ "indexmap",
+ "lazy_static",
+ "log",
+ "phf",
+ "prettyplease",
+ "proc-macro2",
+ "quote",
+ "r2r_actions",
+ "r2r_common",
+ "r2r_macros",
+ "r2r_msg_gen",
+ "r2r_rcl",
+ "rayon",
+ "serde",
+ "serde_json",
+ "syn",
+ "thiserror",
+ "uuid",
+]
+
+[[package]]
+name = "r2r_actions"
+version = "0.9.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5ca8488195951aaadef421b470c823cd095848bebf2004ff96ef5d351067eac1"
+dependencies = [
+ "bindgen 0.71.1",
+ "r2r_common",
+ "r2r_msg_gen",
+ "r2r_rcl",
+]
+
+[[package]]
+name = "r2r_common"
+version = "0.9.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9838c2f88c9fd088f188eadbf9459371b1e576affd7b2df04c45cd1e44b79b99"
+dependencies = [
+ "bindgen 0.71.1",
+ "os_str_bytes",
+ "regex",
+ "sha2",
+]
+
+[[package]]
+name = "r2r_macros"
+version = "0.9.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "be0cda9778c21f78d2d593df41298c14b6530061dc70d2d31eb831f7fec8d648"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "r2r_msg_gen"
+version = "0.9.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "03d5e77385a6b959014c14c45a444e1fbff99afad70912e120c21c30e7a65488"
+dependencies = [
+ "bindgen 0.71.1",
+ "force-send-sync",
+ "itertools 0.10.5",
+ "phf",
+ "proc-macro2",
+ "quote",
+ "r2r_common",
+ "r2r_rcl",
+ "rayon",
+ "syn",
+]
+
+[[package]]
+name = "r2r_rcl"
+version = "0.9.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9e5c208b2fbe64a00a42a70391f708298b6765769d39987286f4b7c82539561e"
+dependencies = [
+ "bindgen 0.71.1",
+ "paste",
+ "r2r_common",
+ "widestring",
+]
+
+[[package]]
+name = "rand"
+version = "0.8.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404"
+dependencies = [
+ "rand_core",
+]
+
+[[package]]
+name = "rand_core"
+version = "0.6.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c"
+
+[[package]]
+name = "rayon"
+version = "1.11.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "368f01d005bf8fd9b1206fb6fa653e6c4a81ceb1466406b81792d87c5677a58f"
+dependencies = [
+ "either",
+ "rayon-core",
+]
+
+[[package]]
+name = "rayon-core"
+version = "1.13.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "22e18b0f0062d30d4230b2e85ff77fdfe4326feb054b9783a3460d8435c8ab91"
+dependencies = [
+ "crossbeam-deque",
+ "crossbeam-utils",
 ]
 
 [[package]]
@@ -566,16 +942,16 @@ dependencies = [
 ]
 
 [[package]]
-name = "ryu"
-version = "1.0.20"
+name = "rustversion"
+version = "1.0.22"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "28d3b2b1366ec20994f1fd18c3c594f05c5dd4bc44d8bb0c1c632c8d6829481f"
+checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d"
 
 [[package]]
-name = "scopeguard"
-version = "1.2.0"
+name = "ryu"
+version = "1.0.20"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
+checksum = "28d3b2b1366ec20994f1fd18c3c594f05c5dd4bc44d8bb0c1c632c8d6829481f"
 
 [[package]]
 name = "serde"
@@ -618,6 +994,17 @@ dependencies = [
  "serde",
 ]
 
+[[package]]
+name = "sha2"
+version = "0.10.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a7507d819769d01a365ab707794a4084392c824f54a7a6a7862f8c3d0892b283"
+dependencies = [
+ "cfg-if",
+ "cpufeatures",
+ "digest",
+]
+
 [[package]]
 name = "shlex"
 version = "1.3.0"
@@ -625,10 +1012,16 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
 
 [[package]]
-name = "smallvec"
-version = "1.15.1"
+name = "siphasher"
+version = "1.0.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "67b1b7a3b5fe4f1376887184045fcf45c69e92af734b7aaddc05fb777b6fbd03"
+checksum = "56199f7ddabf13fe5074ce809e7d3f42b42ae711800501b5b16ea82ad029c39d"
+
+[[package]]
+name = "slab"
+version = "0.4.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7a2ae44ef20feb57a68b23d846850f861394c2e02dc425a50098ae8c90267589"
 
 [[package]]
 name = "strsim"
@@ -666,6 +1059,26 @@ dependencies = [
  "windows-sys 0.59.0",
 ]
 
+[[package]]
+name = "thiserror"
+version = "1.0.69"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b6aaf5339b578ea85b50e080feb250a3e8ae8cfcdff9a461c9ec2904bc923f52"
+dependencies = [
+ "thiserror-impl",
+]
+
+[[package]]
+name = "thiserror-impl"
+version = "1.0.69"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
 [[package]]
 name = "toml"
 version = "0.8.23"
@@ -707,6 +1120,12 @@ version = "0.1.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "5d99f8c9a7727884afe522e9bd5edbfc91a3312b36a77b5fb8926e4c31a41801"
 
+[[package]]
+name = "typenum"
+version = "1.18.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f"
+
 [[package]]
 name = "unicode-ident"
 version = "1.0.18"
@@ -725,6 +1144,24 @@ version = "0.2.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821"
 
+[[package]]
+name = "uuid"
+version = "1.18.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2f87b8aa10b915a06587d0dec516c282ff295b475d94abf425d62b57710070a2"
+dependencies = [
+ "getrandom",
+ "js-sys",
+ "serde",
+ "wasm-bindgen",
+]
+
+[[package]]
+name = "version_check"
+version = "0.9.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a"
+
 [[package]]
 name = "wasi"
 version = "0.14.2+wasi-0.2.4"
@@ -734,6 +1171,71 @@ dependencies = [
  "wit-bindgen-rt",
 ]
 
+[[package]]
+name = "wasm-bindgen"
+version = "0.2.101"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7e14915cadd45b529bb8d1f343c4ed0ac1de926144b746e2710f9cd05df6603b"
+dependencies = [
+ "cfg-if",
+ "once_cell",
+ "rustversion",
+ "wasm-bindgen-macro",
+ "wasm-bindgen-shared",
+]
+
+[[package]]
+name = "wasm-bindgen-backend"
+version = "0.2.101"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e28d1ba982ca7923fd01448d5c30c6864d0a14109560296a162f80f305fb93bb"
+dependencies = [
+ "bumpalo",
+ "log",
+ "proc-macro2",
+ "quote",
+ "syn",
+ "wasm-bindgen-shared",
+]
+
+[[package]]
+name = "wasm-bindgen-macro"
+version = "0.2.101"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7c3d463ae3eff775b0c45df9da45d68837702ac35af998361e2c84e7c5ec1b0d"
+dependencies = [
+ "quote",
+ "wasm-bindgen-macro-support",
+]
+
+[[package]]
+name = "wasm-bindgen-macro-support"
+version = "0.2.101"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7bb4ce89b08211f923caf51d527662b75bdc9c9c7aab40f86dcb9fb85ac552aa"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+ "wasm-bindgen-backend",
+ "wasm-bindgen-shared",
+]
+
+[[package]]
+name = "wasm-bindgen-shared"
+version = "0.2.101"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f143854a3b13752c6950862c906306adb27c7e839f7414cec8fea35beab624c1"
+dependencies = [
+ "unicode-ident",
+]
+
+[[package]]
+name = "widestring"
+version = "1.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dd7cf3379ca1aac9eea11fba24fd7e315d621f8dfe35c8d7d2be8b793726e07d"
+
 [[package]]
 name = "windows-link"
 version = "0.1.3"
diff --git a/src/mission-planner/Cargo.toml b/src/mission-planner/Cargo.toml
index dc5b343..86338fd 100644
--- a/src/mission-planner/Cargo.toml
+++ b/src/mission-planner/Cargo.toml
@@ -4,7 +4,8 @@ version = "0.1.0"
 edition = "2024"
 
 [dependencies]
-dashmap = "6.1.0"
+futures = { version = "0.3.31", features = ["thread-pool"] }
+r2r = "0.9.5"
 
 [dependencies.pyo3]
 version = "0.25.1"
diff --git a/src/mission-planner/bindings.h b/src/mission-planner/bindings.h
index dcd158f..98b3b73 100644
--- a/src/mission-planner/bindings.h
+++ b/src/mission-planner/bindings.h
@@ -13,9 +13,9 @@ typedef struct CMissionPtr CMissionPtr;
 
 typedef struct CTask CTask;
 
-typedef struct MissionMapPtr MissionMapPtr;
+typedef struct MissionDataPtr MissionDataPtr;
 
-typedef enum CMissionResult (*CTaskFunc)(struct MissionMapPtr *data);
+typedef enum CMissionResult (*CTaskFunc)(struct MissionDataPtr *data);
 
 typedef enum OptionFunction_CTaskFunc_Tag {
   Some_CTaskFunc,
diff --git a/src/mission-planner/src/cmission_example.c b/src/mission-planner/src/cmission_example.c
index 62bb81c..73b2a86 100644
--- a/src/mission-planner/src/cmission_example.c
+++ b/src/mission-planner/src/cmission_example.c
@@ -3,8 +3,8 @@
 
 #include "stdio.h"
 
-enum CMissionResult foo(struct MissionMapPtr *data) {
-    printf("Hello world!\n");
+enum CMissionResult foo(struct MissionDataPtr *data) {
+    printf("Hello from C!\n");
     return Ok;
 }
 
diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index affb07d..c8a9289 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -4,13 +4,12 @@ mod cmission;
 mod pymission;
 mod mission_example;
 mod concurrent_mission_example;
-mod ros_sub_mission_example;
 
 include!(concat!(env!("OUT_DIR"), "/bindings.rs"));
 
 use std::vec;
 use std::{collections::VecDeque};
-use std::time::Duration;
+use std::time::{Duration, Instant};
 
 
 use pyo3::{ffi::c_str};
@@ -35,21 +34,21 @@ fn main() {
     let mission_list = VecDeque::from(vec![
         pymission_example,
         cmission_example,
-        foo
+        foo,
     ]);
     let conc_mission_list = VecDeque::from(vec![
         bar
     ]);
 
-    let scheduler = MissionScheduler::start();
+    let mut scheduler = MissionScheduler::start();
     scheduler.append(mission_list);
     scheduler.conc_append(conc_mission_list);
     let data = scheduler.get_data();
 
+    let start = Instant::now();
     scheduler.run();
-    while ! scheduler.is_waiting() {
-        println!("{:#?}", data);
-        std::thread::sleep(Duration::from_secs(1));
+    while start.elapsed() < Duration::from_secs(15) {
+        scheduler.ros_spin();
     }
     scheduler.stop();
 
diff --git a/src/mission-planner/src/mission.rs b/src/mission-planner/src/mission.rs
index 06819f4..45e4130 100644
--- a/src/mission-planner/src/mission.rs
+++ b/src/mission-planner/src/mission.rs
@@ -1,6 +1,4 @@
-use dashmap::DashMap;
-
-pub type MissionHashMap = DashMap<String, String>;
+use std::sync::atomic::AtomicBool;
 pub type MissionResult = Result<(), bool>;
 
 #[derive(Debug)]
diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index f61f964..911e70f 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -1,9 +1,12 @@
-use dashmap::DashMap;
+use futures::StreamExt;
+use futures::executor::ThreadPool;
+use r2r::{Node, QosProfile, std_msgs};
 use crate::mission::{Mission, MissionData};
 use std::collections::VecDeque;
 use std::sync::{Arc, Mutex};
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::thread::{self, sleep};
+use std::time::Duration;
 
 pub type MissionVec = VecDeque<Mission>;
 struct MissionThreadData {
@@ -13,7 +16,6 @@ struct MissionThreadData {
     run: AtomicBool,
     stop: AtomicBool,
     waiting: AtomicBool,
-
 }
 
 impl MissionThreadData {
@@ -44,18 +46,24 @@ impl MissionThreadData {
         self.with_mission_list(func, false)
     }
 }
+
+
 pub struct MissionScheduler {
     normal_handle: thread::JoinHandle<()>,
     concurrent_handle: thread::JoinHandle<()>,
+    node: Node,
+    pool: ThreadPool,
     scheduler_data : Arc<MissionThreadData>,
 }
 
 impl MissionScheduler {
-    fn new(normal_handle: thread::JoinHandle<()>, concurrent_handle: thread::JoinHandle<()>, scheduler_data: Arc<MissionThreadData>) -> Self {
+    fn new(normal_handle: thread::JoinHandle<()>, concurrent_handle: thread::JoinHandle<()>, node: Node, scheduler_data: Arc<MissionThreadData>) -> Self {
         Self {
             normal_handle,
             concurrent_handle,
+            node,
             scheduler_data,
+            pool: ThreadPool::new().expect("Failed to create ThreadPool"),
         }
     }
 
@@ -99,6 +107,7 @@ impl MissionScheduler {
         self.scheduler_data.mission_data.clone()
     }
 
+    #[allow(unused)]
     pub fn is_waiting(&self) -> bool {
         self.scheduler_data.waiting.load(Ordering::Relaxed)
     }
@@ -107,27 +116,70 @@ impl MissionScheduler {
     //     self.concurrent_mission_list.append(mission);
     // }
 
+    fn run_ros_topics(&mut self) {
+        let mut example_sub = self.node
+            .subscribe::<std_msgs::msg::String>("/example", QosProfile::default())
+            .expect("Failed to create example subscriber!");
+        let example_pub= self.node
+            .create_publisher::<std_msgs::msg::String>("/example", QosProfile::default())
+            .expect("Failed to create example publisher!");
+    
+        let scheduler_data = self.scheduler_data.clone();
+        let example_subscriber_func = async move {
+            while ! scheduler_data.stop.load(Ordering::Relaxed) {
+                match example_sub.next().await {
+                    Some(msg) => {
+                        println!("msg: {}", msg.data);
+                    }
+                    None => break,
+                }
+            }
+        };
+
+        let scheduler_data = self.scheduler_data.clone();
+        let example_publisher_func = async move {
+            let mut counter = 0;
+            let mut stop = false;
+            while ! stop {
+                let msg = std_msgs::msg::String {
+                    data: format!("{}", counter),
+                };
+                example_pub.publish(&msg).expect("Failed to publish example!");
+                counter += 1;
+                stop = scheduler_data.stop.load(Ordering::Relaxed);
+                //Should we use a ros timer instead?
+                sleep(Duration::from_secs(1));
+                //This should probably go on another thread
+            }
+        };
+    
+        self.pool.spawn_ok(example_publisher_func);
+        self.pool.spawn_ok(example_subscriber_func);
+    }
+   
     pub fn start() -> Self {
-        let scheduler_data = Arc::new(MissionThreadData::new());
 
-        let scheduler_data_normal = scheduler_data.clone();
+        let scheduler_data_orig = Arc::new(MissionThreadData::new());
+
+
+        let scheduler_data = scheduler_data_orig.clone();
         let normal_func = move || {
             let mut stop = false;
-            let data = &scheduler_data_normal.mission_data;
+            let data = &scheduler_data.mission_data;
 
             while ! stop {
-                let run = scheduler_data_normal.run.load(Ordering::Relaxed);
-                stop = scheduler_data_normal.stop.load(Ordering::Relaxed);
+                let run = scheduler_data.run.load(Ordering::Relaxed);
+                stop = scheduler_data.stop.load(Ordering::Relaxed);
                 if ! run {
                     sleep(std::time::Duration::from_millis(1));
                     continue
                 }
 
 
-                let mission = scheduler_data_normal.pop_front();
+                let mission = scheduler_data.pop_front();
                 let Some(mission) = mission else {
                     println!("Waiting for missions...");
-                    scheduler_data_normal.waiting.store(true, Ordering::Relaxed);
+                    scheduler_data.waiting.store(true, Ordering::Relaxed);
                     sleep(std::time::Duration::from_secs(3));
                     continue;
                 };
@@ -140,7 +192,7 @@ impl MissionScheduler {
                         }
                         else {
                             println!("{} mission failed!", mission.name());
-                            scheduler_data_normal.stop.store(true, Ordering::Relaxed);
+                            scheduler_data.stop.store(true, Ordering::Relaxed);
                             stop = true;
                         }
                     }
@@ -151,14 +203,14 @@ impl MissionScheduler {
             }
         };
 
-        let scheduler_data_conc = scheduler_data.clone();
+        let scheduler_data = scheduler_data_orig.clone();
         let concurrent_func = move || {
             let mut stop = false;
-            let conc_mission_list = scheduler_data_conc.conc_mission_list.clone();
-            let data = &scheduler_data_conc.mission_data;
+            let conc_mission_list = scheduler_data.conc_mission_list.clone();
+            let data = &scheduler_data.mission_data;
             while ! stop {
-                let run = scheduler_data_conc.run.load(Ordering::Relaxed);
-                stop = scheduler_data_conc.stop.load(Ordering::Relaxed);
+                let run = scheduler_data.run.load(Ordering::Relaxed);
+                stop = scheduler_data.stop.load(Ordering::Relaxed);
                 if ! run {
                     sleep(std::time::Duration::from_millis(1));
                     continue
@@ -177,7 +229,7 @@ impl MissionScheduler {
                             }
                             else {
                                 println!("{} mission failed!", mission.name());
-                                scheduler_data_conc.stop.store(true, Ordering::Relaxed);
+                                scheduler_data.stop.store(true, Ordering::Relaxed);
                                 stop = true;
                             }
                         }
@@ -187,12 +239,19 @@ impl MissionScheduler {
                 sleep(std::time::Duration::from_millis(100));
             }
         };
+
+        
         let normal_handle = thread::spawn(normal_func);
         let conc_handle = thread::spawn(concurrent_func);
-        MissionScheduler::new(normal_handle, conc_handle, scheduler_data.clone())
+        let ctx = r2r::Context::create().expect("Failed to create r2r context!");
+        let node = r2r::Node::create(ctx, "mission_scheduler", "namespace")
+            .expect("Failed to get Node!");
+        MissionScheduler::new(normal_handle, conc_handle, node, scheduler_data_orig)
     }
 
-    pub fn run(&self) {
+    pub fn run(&mut self) {
+        self.run_ros_topics();
+
         self.scheduler_data.run.store(true, Ordering::Relaxed);
     }
 
@@ -201,4 +260,8 @@ impl MissionScheduler {
         self.normal_handle.join().expect("Failed to join mission handle!");
         self.concurrent_handle.join().expect("Failed to join concurrent mission handle!");
     }
+
+    pub fn ros_spin(&mut self) {
+        self.node.spin_once(Duration::from_millis(100));
+    }
 }

From 1803485c88f24c55592826dace45b564b753e41d Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Fri, 19 Sep 2025 18:03:03 -0400
Subject: [PATCH 04/19] Example of spawning mission on ros message

---
 src/mission-planner/src/main.rs              |  1 +
 src/mission-planner/src/mission_scheduler.rs | 19 ++++++++++++++++---
 src/mission-planner/src/ros_mission.rs       | 16 ++++++++++++++++
 3 files changed, 33 insertions(+), 3 deletions(-)
 create mode 100644 src/mission-planner/src/ros_mission.rs

diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index c8a9289..7dcf001 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -4,6 +4,7 @@ mod cmission;
 mod pymission;
 mod mission_example;
 mod concurrent_mission_example;
+mod ros_mission;
 
 include!(concat!(env!("OUT_DIR"), "/bindings.rs"));
 
diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index 911e70f..9f68ead 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -7,6 +7,7 @@ use std::sync::{Arc, Mutex};
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::thread::{self, sleep};
 use std::time::Duration;
+use crate::ros_mission;
 
 pub type MissionVec = VecDeque<Mission>;
 struct MissionThreadData {
@@ -45,6 +46,14 @@ impl MissionThreadData {
         };
         self.with_mission_list(func, false)
     }
+
+    pub fn push_back(&self, mission : Mission) -> Option<Mission> {
+        let func = move |mission_list: &mut VecDeque<Mission>| {
+            mission_list.push_back(mission);
+            None
+        };
+        self.with_mission_list(func, false)
+    }
 }
 
 
@@ -117,19 +126,23 @@ impl MissionScheduler {
     // }
 
     fn run_ros_topics(&mut self) {
+        //TODO: We should not have this hardcoded
         let mut example_sub = self.node
-            .subscribe::<std_msgs::msg::String>("/example", QosProfile::default())
+            .subscribe::<std_msgs::msg::String>("/spawn_mission", QosProfile::default())
             .expect("Failed to create example subscriber!");
         let example_pub= self.node
             .create_publisher::<std_msgs::msg::String>("/example", QosProfile::default())
             .expect("Failed to create example publisher!");
-    
+
         let scheduler_data = self.scheduler_data.clone();
         let example_subscriber_func = async move {
             while ! scheduler_data.stop.load(Ordering::Relaxed) {
                 match example_sub.next().await {
                     Some(msg) => {
-                        println!("msg: {}", msg.data);
+                        if msg.data.eq("do-something") {
+                            let mission = ros_mission::new();
+                            scheduler_data.push_back(mission);
+                        }
                     }
                     None => break,
                 }
diff --git a/src/mission-planner/src/ros_mission.rs b/src/mission-planner/src/ros_mission.rs
new file mode 100644
index 0000000..ae32002
--- /dev/null
+++ b/src/mission-planner/src/ros_mission.rs
@@ -0,0 +1,16 @@
+use crate::mission::{Mission, Task, MissionResult, MissionData, RustTask};
+
+fn ros_example(_data: &MissionData) -> MissionResult {
+    println!("Ros mission invoked!");
+    Ok(())
+}
+
+pub fn new() -> Mission {
+    let name = "ros-example-mission".to_string();
+    let task = RustTask::new("ros-example-task".to_string(), Some(ros_example), None);
+    let task_list: Vec<Box<dyn Task>> = vec![
+        Box::new(task)
+    ];
+
+    Mission { name, task_list }
+}

From db0bacb7bebd4e6697f52a9f7a7991943aaac088 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Sun, 21 Sep 2025 08:18:45 -0400
Subject: [PATCH 05/19] mission-planner: Do detailed example with image
 subscriber

---
 .gitmodules                                   |   7 +
 Cargo.lock                                    | 469 ++++++++++++------
 Cargo.toml                                    |   4 +-
 src/mission-planner/Cargo.toml                |   4 +-
 src/mission-planner/src/cmission.rs           |   4 +-
 .../src/concurrent_mission_example.rs         |   6 +-
 src/mission-planner/src/main.rs               |  24 +-
 src/mission-planner/src/mission.rs            |  16 +-
 src/mission-planner/src/mission_example.rs    |   6 +-
 src/mission-planner/src/mission_scheduler.rs  |  43 +-
 src/mission-planner/src/pymission.rs          |   6 +-
 src/mission-planner/src/ros_mission.rs        |  50 +-
 12 files changed, 416 insertions(+), 223 deletions(-)

diff --git a/.gitmodules b/.gitmodules
index 9a53575..1470d33 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -4,3 +4,10 @@
 [submodule "third_party/sam2"]
 	path = third_party/sam2
 	url = https://github.com/facebookresearch/sam2.git
+[submodule "third_party/cv-bridge-rs"]
+	path = third_party/cv-bridge-rs
+	url = https://github.com/ultra-azu/cv-bridge-rs.git
+[submodule "third_party/image_transport_tutorials"]
+	path = third_party/image_transport_tutorials
+	url = https://github.com/ros-perception/image_transport_tutorials.git
+	branch = humble
diff --git a/Cargo.lock b/Cargo.lock
index 248127e..889d005 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -89,9 +89,9 @@ dependencies = [
 
 [[package]]
 name = "bindgen"
-version = "0.72.0"
+version = "0.72.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4f72209734318d0b619a5e0f5129918b848c416e122a3c4ce054e03cb87b726f"
+checksum = "993776b509cfb49c750f11b8f07a46fa23e0a1386ffc01fb1e7d343efc387895"
 dependencies = [
  "bitflags",
  "cexpr",
@@ -109,9 +109,9 @@ dependencies = [
 
 [[package]]
 name = "bitflags"
-version = "2.9.1"
+version = "2.9.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967"
+checksum = "2261d10cca569e4643e526d8dc2e62e433cc8aba21ab764233731f8d369bf394"
 
 [[package]]
 name = "block-buffer"
@@ -128,6 +128,12 @@ version = "3.19.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "46c5e41b57b8bba42a04676d81cb89e9ee8e859a1a66f80a5a72e1cb76b34d43"
 
+[[package]]
+name = "byteorder"
+version = "1.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b"
+
 [[package]]
 name = "cbindgen"
 version = "0.29.0"
@@ -149,10 +155,13 @@ dependencies = [
 
 [[package]]
 name = "cc"
-version = "1.2.31"
+version = "1.2.38"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c3a42d84bb6b69d3a8b3eaacf0d88f179e1929695e1ad012b6cf64d9caaa5fd2"
+checksum = "80f41ae168f955c12fb8960b057d70d0ca153fb83182b57d86380443527be7e9"
 dependencies = [
+ "find-msvc-tools",
+ "jobserver",
+ "libc",
  "shlex",
 ]
 
@@ -167,9 +176,19 @@ dependencies = [
 
 [[package]]
 name = "cfg-if"
-version = "1.0.1"
+version = "1.0.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9555578bc9e57714c812a1f84e4fc5b4d21fcb063490c624de019f7464c91268"
+checksum = "2fd1289c04a9ea8cb22300a459a72a385d7c73d3259e2ed7dcb2af674838cfa9"
+
+[[package]]
+name = "clang"
+version = "2.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "84c044c781163c001b913cd018fc95a628c50d0d2dfea8bca77dad71edb16e37"
+dependencies = [
+ "clang-sys",
+ "libc",
+]
 
 [[package]]
 name = "clang-sys"
@@ -184,18 +203,18 @@ dependencies = [
 
 [[package]]
 name = "clap"
-version = "4.5.43"
+version = "4.5.48"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "50fd97c9dc2399518aa331917ac6f274280ec5eb34e555dd291899745c48ec6f"
+checksum = "e2134bb3ea021b78629caa971416385309e0131b351b25e01dc16fb54e1b5fae"
 dependencies = [
  "clap_builder",
 ]
 
 [[package]]
 name = "clap_builder"
-version = "4.5.43"
+version = "4.5.48"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c35b5830294e1fa0462034af85cc95225a4cb07092c088c55bda3147cfcd8f65"
+checksum = "c2ba64afa3c0a6df7fa517765e31314e983f51dda798ffba27b988194fb65dc9"
 dependencies = [
  "anstream",
  "anstyle",
@@ -259,6 +278,15 @@ dependencies = [
  "typenum",
 ]
 
+[[package]]
+name = "cv-bridge"
+version = "0.3.4"
+dependencies = [
+ "byteorder",
+ "opencv",
+ "r2r",
+]
+
 [[package]]
 name = "digest"
 version = "0.10.7"
@@ -269,6 +297,12 @@ dependencies = [
  "crypto-common",
 ]
 
+[[package]]
+name = "dunce"
+version = "1.0.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "92773504d58c093f6de2459af4af33faa518c13451eb8f2b5698ed3d36e7c813"
+
 [[package]]
 name = "either"
 version = "1.15.0"
@@ -283,12 +317,12 @@ checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f"
 
 [[package]]
 name = "errno"
-version = "0.3.13"
+version = "0.3.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "778e2ac28f6c47af28e4907f13ffd1e1ddbd400980a9abd7c8df189bf578a5ad"
+checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb"
 dependencies = [
  "libc",
- "windows-sys 0.60.2",
+ "windows-sys 0.61.0",
 ]
 
 [[package]]
@@ -297,6 +331,12 @@ version = "2.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "37909eebbb50d72f9059c3b6d82c0463f2ff062c9e95845c43a6c9c0355411be"
 
+[[package]]
+name = "find-msvc-tools"
+version = "0.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1ced73b1dacfc750a6db6c0a0c3a3853c8b41997e2e2c563dc90804ae6867959"
+
 [[package]]
 name = "force-send-sync"
 version = "1.2.0"
@@ -417,15 +457,15 @@ dependencies = [
 
 [[package]]
 name = "glob"
-version = "0.3.2"
+version = "0.3.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2"
+checksum = "0cc23270f6e1808e30a928bdc84dea0b9b4136a8bc82338574f23baf47bbd280"
 
 [[package]]
 name = "hashbrown"
-version = "0.15.4"
+version = "0.16.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5971ac85611da7067dbfcabef3c70ebb5606018acd9e2a3903a0da507521e0d5"
+checksum = "5419bdc4f6a9207fbeba6d11b604d481addf78ecd10c11ad51e76c2f6482748d"
 
 [[package]]
 name = "heck"
@@ -441,9 +481,9 @@ checksum = "fc0fef456e4baa96da950455cd02c081ca953b141298e41db3fc7e36b1da849c"
 
 [[package]]
 name = "indexmap"
-version = "2.10.0"
+version = "2.11.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fe4cd85333e22411419a0bcae1297d25e58c9443848b11dc6a86fefe8c78a661"
+checksum = "4b0f83760fb341a774ed326568e19f5a863af4a952def8c39f9ab92fd95b88e5"
 dependencies = [
  "equivalent",
  "hashbrown",
@@ -485,11 +525,21 @@ version = "1.0.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "4a5f13b858c8d314ee3e8f639011f7ccefe71f97f96e50151fb991f267928e2c"
 
+[[package]]
+name = "jobserver"
+version = "0.1.34"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9afb3de4395d6b3e67a780b6de64b51c978ecf11cb9a462c66be7d4ca9039d33"
+dependencies = [
+ "getrandom",
+ "libc",
+]
+
 [[package]]
 name = "js-sys"
-version = "0.3.78"
+version = "0.3.80"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0c0b063578492ceec17683ef2f8c5e89121fbd0b172cbc280635ab7567db2738"
+checksum = "852f13bec5eba4ba9afbeb93fd7c13fe56147f055939ae21c43a29a0ecb2702e"
 dependencies = [
  "once_cell",
  "wasm-bindgen",
@@ -503,9 +553,9 @@ checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe"
 
 [[package]]
 name = "libc"
-version = "0.2.174"
+version = "0.2.175"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1171693293099992e19cddea4e8b849964e9846f4acee11b3948bcc337be8776"
+checksum = "6a82ae493e598baaea5209805c49bbf2ea7de956d50d7da0da1164f9c6d28543"
 
 [[package]]
 name = "libloading"
@@ -514,20 +564,20 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667"
 dependencies = [
  "cfg-if",
- "windows-targets 0.53.3",
+ "windows-targets",
 ]
 
 [[package]]
 name = "linux-raw-sys"
-version = "0.9.4"
+version = "0.11.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cd945864f07fe9f5371a27ad7b52a172b4b499999f1d97574c9fa68373937e12"
+checksum = "df1d3c3b53da64cf5760482273a98e575c651a67eec7f77df96b5b642de8f039"
 
 [[package]]
 name = "log"
-version = "0.4.27"
+version = "0.4.28"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
+checksum = "34080505efa8e45a4b816c349525ebe327ceaa8559756f0356cba97ef3bf7432"
 
 [[package]]
 name = "memchr"
@@ -554,10 +604,12 @@ checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a"
 name = "mission_planner"
 version = "0.1.0"
 dependencies = [
- "bindgen 0.72.0",
+ "bindgen 0.72.1",
  "cbindgen",
  "cc",
+ "cv-bridge",
  "futures",
+ "opencv",
  "pyo3",
  "r2r",
 ]
@@ -572,6 +624,15 @@ dependencies = [
  "minimal-lexical",
 ]
 
+[[package]]
+name = "num-traits"
+version = "0.2.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841"
+dependencies = [
+ "autocfg",
+]
+
 [[package]]
 name = "num_cpus"
 version = "1.17.0"
@@ -594,6 +655,41 @@ version = "1.70.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a4895175b425cb1f87721b59f0f286c2092bd4af812243672510e1ac53e2e0ad"
 
+[[package]]
+name = "opencv"
+version = "0.95.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0c73b6fccd78797a87cdb885c997351a1a290b0ebde778e996b694dec2a4c04a"
+dependencies = [
+ "cc",
+ "dunce",
+ "jobserver",
+ "libc",
+ "num-traits",
+ "once_cell",
+ "opencv-binding-generator",
+ "pkg-config",
+ "semver",
+ "shlex",
+ "vcpkg",
+ "windows",
+]
+
+[[package]]
+name = "opencv-binding-generator"
+version = "0.97.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "010a78e4cc47ff85cf58fb1cbbbab9dcdb8e5e6718917eac26623f077872d012"
+dependencies = [
+ "clang",
+ "clang-sys",
+ "dunce",
+ "once_cell",
+ "percent-encoding",
+ "regex",
+ "shlex",
+]
+
 [[package]]
 name = "os_str_bytes"
 version = "6.6.1"
@@ -609,6 +705,12 @@ version = "1.0.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
 
+[[package]]
+name = "percent-encoding"
+version = "2.3.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9b4f627cb1b25917193a259e49bdad08f671f8d9708acfd5fe0a8c1455d87220"
+
 [[package]]
 name = "phf"
 version = "0.11.3"
@@ -663,6 +765,12 @@ version = "0.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
 
+[[package]]
+name = "pkg-config"
+version = "0.3.32"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7edddbd0b52d732b21ad9a5fab5c704c14cd949e5e9a1ec5929a24fded1b904c"
+
 [[package]]
 name = "portable-atomic"
 version = "1.11.1"
@@ -671,9 +779,9 @@ checksum = "f84267b20a16ea918e43c6a88433c2d54fa145c92a811b5b047ccbe153674483"
 
 [[package]]
 name = "prettyplease"
-version = "0.2.36"
+version = "0.2.37"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ff24dfcda44452b9816fff4cd4227e1bb73ff5a2f1bc1105aa92fb8565ce44d2"
+checksum = "479ca8adacdd7ce8f1fb39ce9ecccbfe93a3f1344b3d0d97f20bc0196208f62b"
 dependencies = [
  "proc-macro2",
  "syn",
@@ -681,18 +789,18 @@ dependencies = [
 
 [[package]]
 name = "proc-macro2"
-version = "1.0.95"
+version = "1.0.101"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "02b3e5e68a3a1a02aad3ec490a98007cbc13c37cbe84a3cd7b8e406d76e7f778"
+checksum = "89ae43fd86e4158d6db51ad8e2b80f313af9cc74f5c0e03ccb87de09998732de"
 dependencies = [
  "unicode-ident",
 ]
 
 [[package]]
 name = "pyo3"
-version = "0.25.1"
+version = "0.26.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8970a78afe0628a3e3430376fc5fd76b6b45c4d43360ffd6cdd40bdde72b682a"
+checksum = "7ba0117f4212101ee6544044dae45abe1083d30ce7b29c4b5cbdfa2354e07383"
 dependencies = [
  "indoc",
  "libc",
@@ -707,19 +815,18 @@ dependencies = [
 
 [[package]]
 name = "pyo3-build-config"
-version = "0.25.1"
+version = "0.26.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "458eb0c55e7ece017adeba38f2248ff3ac615e53660d7c71a238d7d2a01c7598"
+checksum = "4fc6ddaf24947d12a9aa31ac65431fb1b851b8f4365426e182901eabfb87df5f"
 dependencies = [
- "once_cell",
  "target-lexicon",
 ]
 
 [[package]]
 name = "pyo3-ffi"
-version = "0.25.1"
+version = "0.26.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7114fe5457c61b276ab77c5055f206295b812608083644a5c5b2640c3102565c"
+checksum = "025474d3928738efb38ac36d4744a74a400c901c7596199e20e45d98eb194105"
 dependencies = [
  "libc",
  "pyo3-build-config",
@@ -727,9 +834,9 @@ dependencies = [
 
 [[package]]
 name = "pyo3-macros"
-version = "0.25.1"
+version = "0.26.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a8725c0a622b374d6cb051d11a0983786448f7785336139c3c94f5aa6bef7e50"
+checksum = "2e64eb489f22fe1c95911b77c44cc41e7c19f3082fc81cce90f657cdc42ffded"
 dependencies = [
  "proc-macro2",
  "pyo3-macros-backend",
@@ -739,9 +846,9 @@ dependencies = [
 
 [[package]]
 name = "pyo3-macros-backend"
-version = "0.25.1"
+version = "0.26.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4109984c22491085343c05b0dbc54ddc405c3cf7b4374fc533f5c3313a572ccc"
+checksum = "100246c0ecf400b475341b8455a9213344569af29a3c841d29270e53102e0fcf"
 dependencies = [
  "heck",
  "proc-macro2",
@@ -895,9 +1002,9 @@ dependencies = [
 
 [[package]]
 name = "regex"
-version = "1.11.1"
+version = "1.11.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191"
+checksum = "23d7fd106d8c02486a8d64e778353d1cffe08ce79ac2e82f540c86d0facf6912"
 dependencies = [
  "aho-corasick",
  "memchr",
@@ -907,9 +1014,9 @@ dependencies = [
 
 [[package]]
 name = "regex-automata"
-version = "0.4.9"
+version = "0.4.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908"
+checksum = "6b9458fa0bfeeac22b5ca447c63aaf45f28439a709ccd244698632f9aa6394d6"
 dependencies = [
  "aho-corasick",
  "memchr",
@@ -918,9 +1025,9 @@ dependencies = [
 
 [[package]]
 name = "regex-syntax"
-version = "0.8.5"
+version = "0.8.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c"
+checksum = "caf4aa5b0f434c91fe5c7f1ecb6a5ece2130b02ad2a590589dda5146df959001"
 
 [[package]]
 name = "rustc-hash"
@@ -930,15 +1037,15 @@ checksum = "357703d41365b4b27c590e3ed91eabb1b663f07c4c084095e60cbed4362dff0d"
 
 [[package]]
 name = "rustix"
-version = "1.0.8"
+version = "1.1.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "11181fbabf243db407ef8df94a6ce0b2f9a733bd8be4ad02b4eda9602296cac8"
+checksum = "cd15f8a2c5551a84d56efdc1cd049089e409ac19a3072d5037a17fd70719ff3e"
 dependencies = [
  "bitflags",
  "errno",
  "libc",
  "linux-raw-sys",
- "windows-sys 0.60.2",
+ "windows-sys 0.61.0",
 ]
 
 [[package]]
@@ -953,20 +1060,36 @@ version = "1.0.20"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "28d3b2b1366ec20994f1fd18c3c594f05c5dd4bc44d8bb0c1c632c8d6829481f"
 
+[[package]]
+name = "semver"
+version = "1.0.27"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d767eb0aabc880b29956c35734170f26ed551a859dbd361d140cdbeca61ab1e2"
+
 [[package]]
 name = "serde"
-version = "1.0.219"
+version = "1.0.225"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6"
+checksum = "fd6c24dee235d0da097043389623fb913daddf92c76e9f5a1db88607a0bcbd1d"
+dependencies = [
+ "serde_core",
+ "serde_derive",
+]
+
+[[package]]
+name = "serde_core"
+version = "1.0.225"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "659356f9a0cb1e529b24c01e43ad2bdf520ec4ceaf83047b83ddcc2251f96383"
 dependencies = [
  "serde_derive",
 ]
 
 [[package]]
 name = "serde_derive"
-version = "1.0.219"
+version = "1.0.225"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00"
+checksum = "0ea936adf78b1f766949a4977b91d2f5595825bd6ec079aa9543ad2685fc4516"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -975,14 +1098,15 @@ dependencies = [
 
 [[package]]
 name = "serde_json"
-version = "1.0.142"
+version = "1.0.145"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "030fedb782600dcbd6f02d479bf0d817ac3bb40d644745b769d6a96bc3afc5a7"
+checksum = "402a6f66d8c709116cf22f558eab210f5a50187f702eb4d7e5ef38d9a7f1c79c"
 dependencies = [
  "itoa",
  "memchr",
  "ryu",
  "serde",
+ "serde_core",
 ]
 
 [[package]]
@@ -1031,9 +1155,9 @@ checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f"
 
 [[package]]
 name = "syn"
-version = "2.0.104"
+version = "2.0.106"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "17b6f705963418cdb9927482fa304bc562ece2fdd4f616084c50b7023b435a40"
+checksum = "ede7c438028d4436d71104916910f5bb611972c5cfd7f89b8300a8186e6fada6"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -1042,21 +1166,21 @@ dependencies = [
 
 [[package]]
 name = "target-lexicon"
-version = "0.13.2"
+version = "0.13.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e502f78cdbb8ba4718f566c418c52bc729126ffd16baee5baa718cf25dd5a69a"
+checksum = "df7f62577c25e07834649fc3b39fafdc597c0a3527dc1c60129201ccfcbaa50c"
 
 [[package]]
 name = "tempfile"
-version = "3.20.0"
+version = "3.22.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e8a64e3985349f2441a1a9ef0b853f869006c3855f2cda6862a94d26ebb9d6a1"
+checksum = "84fa4d11fadde498443cca10fd3ac23c951f0dc59e080e9f4b93d4df4e4eea53"
 dependencies = [
  "fastrand",
  "getrandom",
  "once_cell",
  "rustix",
- "windows-sys 0.59.0",
+ "windows-sys 0.61.0",
 ]
 
 [[package]]
@@ -1128,9 +1252,9 @@ checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f"
 
 [[package]]
 name = "unicode-ident"
-version = "1.0.18"
+version = "1.0.19"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512"
+checksum = "f63a545481291138910575129486daeaf8ac54aee4387fe7906919f7830c7d9d"
 
 [[package]]
 name = "unindent"
@@ -1156,6 +1280,12 @@ dependencies = [
  "wasm-bindgen",
 ]
 
+[[package]]
+name = "vcpkg"
+version = "0.2.15"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "accd4ea62f7bb7a82fe23066fb0957d48ef677f6eeb8215f372f52e48bb32426"
+
 [[package]]
 name = "version_check"
 version = "0.9.5"
@@ -1164,18 +1294,27 @@ checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a"
 
 [[package]]
 name = "wasi"
-version = "0.14.2+wasi-0.2.4"
+version = "0.14.7+wasi-0.2.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9683f9a5a998d873c0d21fcbe3c083009670149a8fab228644b8bd36b2c48cb3"
+checksum = "883478de20367e224c0090af9cf5f9fa85bed63a95c1abf3afc5c083ebc06e8c"
 dependencies = [
- "wit-bindgen-rt",
+ "wasip2",
+]
+
+[[package]]
+name = "wasip2"
+version = "1.0.1+wasi-0.2.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0562428422c63773dad2c345a1882263bbf4d65cf3f42e90921f787ef5ad58e7"
+dependencies = [
+ "wit-bindgen",
 ]
 
 [[package]]
 name = "wasm-bindgen"
-version = "0.2.101"
+version = "0.2.103"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7e14915cadd45b529bb8d1f343c4ed0ac1de926144b746e2710f9cd05df6603b"
+checksum = "ab10a69fbd0a177f5f649ad4d8d3305499c42bab9aef2f7ff592d0ec8f833819"
 dependencies = [
  "cfg-if",
  "once_cell",
@@ -1186,9 +1325,9 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen-backend"
-version = "0.2.101"
+version = "0.2.103"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e28d1ba982ca7923fd01448d5c30c6864d0a14109560296a162f80f305fb93bb"
+checksum = "0bb702423545a6007bbc368fde243ba47ca275e549c8a28617f56f6ba53b1d1c"
 dependencies = [
  "bumpalo",
  "log",
@@ -1200,9 +1339,9 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen-macro"
-version = "0.2.101"
+version = "0.2.103"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7c3d463ae3eff775b0c45df9da45d68837702ac35af998361e2c84e7c5ec1b0d"
+checksum = "fc65f4f411d91494355917b605e1480033152658d71f722a90647f56a70c88a0"
 dependencies = [
  "quote",
  "wasm-bindgen-macro-support",
@@ -1210,9 +1349,9 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen-macro-support"
-version = "0.2.101"
+version = "0.2.103"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7bb4ce89b08211f923caf51d527662b75bdc9c9c7aab40f86dcb9fb85ac552aa"
+checksum = "ffc003a991398a8ee604a401e194b6b3a39677b3173d6e74495eb51b82e99a32"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -1223,9 +1362,9 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen-shared"
-version = "0.2.101"
+version = "0.2.103"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f143854a3b13752c6950862c906306adb27c7e839f7414cec8fea35beab624c1"
+checksum = "293c37f4efa430ca14db3721dfbe48d8c33308096bd44d80ebaa775ab71ba1cf"
 dependencies = [
  "unicode-ident",
 ]
@@ -1236,6 +1375,51 @@ version = "1.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "dd7cf3379ca1aac9eea11fba24fd7e315d621f8dfe35c8d7d2be8b793726e07d"
 
+[[package]]
+name = "windows"
+version = "0.59.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7f919aee0a93304be7f62e8e5027811bbba96bcb1de84d6618be56e43f8a32a1"
+dependencies = [
+ "windows-core",
+ "windows-targets",
+]
+
+[[package]]
+name = "windows-core"
+version = "0.59.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "810ce18ed2112484b0d4e15d022e5f598113e220c53e373fb31e67e21670c1ce"
+dependencies = [
+ "windows-implement",
+ "windows-interface",
+ "windows-result",
+ "windows-strings",
+ "windows-targets",
+]
+
+[[package]]
+name = "windows-implement"
+version = "0.59.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "83577b051e2f49a058c308f17f273b570a6a758386fc291b5f6a934dd84e48c1"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "windows-interface"
+version = "0.59.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bd9211b69f8dcdfa817bfd14bf1c97c9188afa36f4750130fcdf3f400eca9fa8"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
 [[package]]
 name = "windows-link"
 version = "0.1.3"
@@ -1243,12 +1427,27 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "5e6ad25900d524eaabdbbb96d20b4311e1e7ae1699af4fb28c17ae66c80d798a"
 
 [[package]]
-name = "windows-sys"
-version = "0.59.0"
+name = "windows-link"
+version = "0.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b"
+checksum = "45e46c0661abb7180e7b9c281db115305d49ca1709ab8242adf09666d2173c65"
+
+[[package]]
+name = "windows-result"
+version = "0.3.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "56f42bd332cc6c8eac5af113fc0c1fd6a8fd2aa08a0119358686e5160d0586c6"
 dependencies = [
- "windows-targets 0.52.6",
+ "windows-link 0.1.3",
+]
+
+[[package]]
+name = "windows-strings"
+version = "0.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "87fa48cc5d406560701792be122a10132491cff9d0aeb23583cc2dcafc847319"
+dependencies = [
+ "windows-link 0.1.3",
 ]
 
 [[package]]
@@ -1257,23 +1456,16 @@ version = "0.60.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "f2f500e4d28234f72040990ec9d39e3a6b950f9f22d3dba18416c35882612bcb"
 dependencies = [
- "windows-targets 0.53.3",
+ "windows-targets",
 ]
 
 [[package]]
-name = "windows-targets"
-version = "0.52.6"
+name = "windows-sys"
+version = "0.61.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973"
+checksum = "e201184e40b2ede64bc2ea34968b28e33622acdbbf37104f0e4a33f7abe657aa"
 dependencies = [
- "windows_aarch64_gnullvm 0.52.6",
- "windows_aarch64_msvc 0.52.6",
- "windows_i686_gnu 0.52.6",
- "windows_i686_gnullvm 0.52.6",
- "windows_i686_msvc 0.52.6",
- "windows_x86_64_gnu 0.52.6",
- "windows_x86_64_gnullvm 0.52.6",
- "windows_x86_64_msvc 0.52.6",
+ "windows-link 0.2.0",
 ]
 
 [[package]]
@@ -1282,107 +1474,59 @@ version = "0.53.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d5fe6031c4041849d7c496a8ded650796e7b6ecc19df1a431c1a363342e5dc91"
 dependencies = [
- "windows-link",
- "windows_aarch64_gnullvm 0.53.0",
- "windows_aarch64_msvc 0.53.0",
- "windows_i686_gnu 0.53.0",
- "windows_i686_gnullvm 0.53.0",
- "windows_i686_msvc 0.53.0",
- "windows_x86_64_gnu 0.53.0",
- "windows_x86_64_gnullvm 0.53.0",
- "windows_x86_64_msvc 0.53.0",
+ "windows-link 0.1.3",
+ "windows_aarch64_gnullvm",
+ "windows_aarch64_msvc",
+ "windows_i686_gnu",
+ "windows_i686_gnullvm",
+ "windows_i686_msvc",
+ "windows_x86_64_gnu",
+ "windows_x86_64_gnullvm",
+ "windows_x86_64_msvc",
 ]
 
-[[package]]
-name = "windows_aarch64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3"
-
 [[package]]
 name = "windows_aarch64_gnullvm"
 version = "0.53.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764"
 
-[[package]]
-name = "windows_aarch64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469"
-
 [[package]]
 name = "windows_aarch64_msvc"
 version = "0.53.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c"
 
-[[package]]
-name = "windows_i686_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b"
-
 [[package]]
 name = "windows_i686_gnu"
 version = "0.53.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3"
 
-[[package]]
-name = "windows_i686_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66"
-
 [[package]]
 name = "windows_i686_gnullvm"
 version = "0.53.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11"
 
-[[package]]
-name = "windows_i686_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66"
-
 [[package]]
 name = "windows_i686_msvc"
 version = "0.53.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d"
 
-[[package]]
-name = "windows_x86_64_gnu"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78"
-
 [[package]]
 name = "windows_x86_64_gnu"
 version = "0.53.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba"
 
-[[package]]
-name = "windows_x86_64_gnullvm"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d"
-
 [[package]]
 name = "windows_x86_64_gnullvm"
 version = "0.53.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57"
 
-[[package]]
-name = "windows_x86_64_msvc"
-version = "0.52.6"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec"
-
 [[package]]
 name = "windows_x86_64_msvc"
 version = "0.53.0"
@@ -1391,18 +1535,15 @@ checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486"
 
 [[package]]
 name = "winnow"
-version = "0.7.12"
+version = "0.7.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f3edebf492c8125044983378ecb5766203ad3b4c2f7a922bd7dd207f6d443e95"
+checksum = "21a0236b59786fed61e2a80582dd500fe61f18b5dca67a4a067d0bc9039339cf"
 dependencies = [
  "memchr",
 ]
 
 [[package]]
-name = "wit-bindgen-rt"
-version = "0.39.0"
+name = "wit-bindgen"
+version = "0.46.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6f42320e61fe2cfd34354ecb597f86f413484a798ba44a8ca1165c58d42da6c1"
-dependencies = [
- "bitflags",
-]
+checksum = "f17a85883d4e6d00e8a97c586de764dabcc06133f7f1d55dce5cdc070ad7fe59"
diff --git a/Cargo.toml b/Cargo.toml
index 6545900..dfec0e3 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -2,5 +2,5 @@
 resolver = "3"
 
 members = [
-    "src/mission-planner"
-]
+    "src/mission-planner",
+    "third_party/cv-bridge-rs"]
diff --git a/src/mission-planner/Cargo.toml b/src/mission-planner/Cargo.toml
index 86338fd..b9f2dff 100644
--- a/src/mission-planner/Cargo.toml
+++ b/src/mission-planner/Cargo.toml
@@ -4,11 +4,13 @@ version = "0.1.0"
 edition = "2024"
 
 [dependencies]
+cv-bridge = { version = "0.3.4", path = "../../third_party/cv-bridge-rs" }
 futures = { version = "0.3.31", features = ["thread-pool"] }
+opencv = { version = "0.95.1", features = ["clang-runtime"] }
 r2r = "0.9.5"
 
 [dependencies.pyo3]
-version = "0.25.1"
+version = "0.26.0"
 features = ["auto-initialize"]
 
 [build-dependencies]
diff --git a/src/mission-planner/src/cmission.rs b/src/mission-planner/src/cmission.rs
index 2c1c93b..145e2db 100644
--- a/src/mission-planner/src/cmission.rs
+++ b/src/mission-planner/src/cmission.rs
@@ -1,4 +1,4 @@
-use crate::mission::{Mission, MissionData, MissionResult, Task};
+use crate::mission::{CommonMission, MissionData, MissionResult, Task};
 use std::ffi::{c_char, CStr};
 
 #[repr(C)]
@@ -97,7 +97,7 @@ pub extern "C" fn cmission_create(name_ptr: *const c_char, task_array: *mut CTas
         name = temp.to_str().expect("Failed to get string literal!");
     };
     let name = name.to_string();
-    let mission = Mission {
+    let mission = CommonMission {
         name,
         task_list
     };
diff --git a/src/mission-planner/src/concurrent_mission_example.rs b/src/mission-planner/src/concurrent_mission_example.rs
index 4673ded..a054ef4 100644
--- a/src/mission-planner/src/concurrent_mission_example.rs
+++ b/src/mission-planner/src/concurrent_mission_example.rs
@@ -1,6 +1,6 @@
 use std::sync::atomic::Ordering;
 
-use crate::mission::{Mission, MissionResult, MissionData, RustTask, Task};
+use crate::mission::{CommonMission, Mission, MissionData, MissionResult, RustTask, Task};
 
 fn conc_example(data: &MissionData) -> MissionResult {
     if data.example_flag_request.load(Ordering::Relaxed) {
@@ -10,12 +10,12 @@ fn conc_example(data: &MissionData) -> MissionResult {
     Ok(())
 }
 
-pub fn new() -> Mission {
+pub fn new() -> impl Mission {
     let name = "concurrent-mission-example".to_string();
     let task = RustTask::new("conc-example-task".to_string(), Some(conc_example), None);
     let task_list: Vec<Box<dyn Task>> =  vec![
         Box::new(task)
     ];
-    Mission { name, task_list }
+    CommonMission { name, task_list }
 }
 
diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index 7dcf001..5ceedb3 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -15,10 +15,12 @@ use std::time::{Duration, Instant};
 
 use pyo3::{ffi::c_str};
 
+use crate::mission_scheduler::{MissionBox, MissionVec};
 use crate::{
-    mission::{Mission}, mission_scheduler::MissionScheduler
+    mission::{CommonMission}, mission_scheduler::MissionScheduler
 };
 
+
 fn main() {
     let pytest = c_str!(include_str!("pymission_example.py"));
     let pymission_example = pymission::get_mission_from(pytest, c_str!("pymission_example.py"));
@@ -26,20 +28,22 @@ fn main() {
     let cmission_example;
     unsafe {
         let cmission_ptr = cmission_example_create();
-        cmission_example = *Box::from_raw(cmission_ptr as *mut Mission);
+        cmission_example = *Box::from_raw(cmission_ptr as *mut CommonMission);
     }
 
     let foo = mission_example::new();
     let bar = concurrent_mission_example::new();
 
-    let mission_list = VecDeque::from(vec![
-        pymission_example,
-        cmission_example,
-        foo,
-    ]);
-    let conc_mission_list = VecDeque::from(vec![
-        bar
-    ]);
+    let mission_list: [MissionBox; 3] = [
+        Box::new(pymission_example),
+        Box::new(cmission_example),
+        Box::new(foo),
+    ];
+    let mission_list = VecDeque::from(mission_list);
+    let conc_mission_list: [MissionBox; 1] = [
+        Box::new(bar)
+    ];
+    let conc_mission_list = VecDeque::from(conc_mission_list);
 
     let mut scheduler = MissionScheduler::start();
     scheduler.append(mission_list);
diff --git a/src/mission-planner/src/mission.rs b/src/mission-planner/src/mission.rs
index 45e4130..d8d5429 100644
--- a/src/mission-planner/src/mission.rs
+++ b/src/mission-planner/src/mission.rs
@@ -59,13 +59,21 @@ impl Task for RustTask {
     }
 }
 
-pub struct Mission {
+pub struct CommonMission {
     pub name: String,
     pub task_list: Vec<Box<dyn Task>>,
 }
 
-impl Mission {
-    pub fn run(&self, data: &MissionData) -> MissionResult {
+pub trait Mission : Send + Sync {
+    fn run(&self, data: &MissionData) -> MissionResult;
+    fn name(&self) -> &String;
+}
+
+impl CommonMission {
+    
+}
+impl Mission for CommonMission {
+    fn run(&self, data: &MissionData) -> MissionResult {
         if self.task_list.is_empty() {
             return Ok(())
         }
@@ -93,7 +101,7 @@ impl Mission {
         res
     }
 
-    pub fn name(&self) -> &String {
+    fn name(&self) -> &String {
         &self.name
     }
 }
diff --git a/src/mission-planner/src/mission_example.rs b/src/mission-planner/src/mission_example.rs
index 7995981..e13ac30 100644
--- a/src/mission-planner/src/mission_example.rs
+++ b/src/mission-planner/src/mission_example.rs
@@ -1,5 +1,5 @@
 use std::thread::sleep;
-use crate::mission::{Mission, Task, MissionResult, MissionData, RustTask};
+use crate::mission::{CommonMission, Mission, MissionData, MissionResult, RustTask, Task};
 use std::sync::atomic::Ordering;
 
 fn example(_data: &MissionData) -> MissionResult {
@@ -19,12 +19,12 @@ fn repair_example(data: &MissionData) -> MissionResult {
     Ok(())
 }
 
-pub fn new() -> Mission {
+pub fn new() -> impl Mission {
     let name = "example-mission".to_string();
     let task = RustTask::new("example-task".to_string(), Some(example), Some(repair_example));
     let task_list: Vec<Box<dyn Task>> = vec![
         Box::new(task)
     ];
 
-    Mission { name, task_list }
+    CommonMission { name, task_list }
 }
diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index 9f68ead..d88c71f 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -1,7 +1,7 @@
 use futures::StreamExt;
 use futures::executor::ThreadPool;
-use r2r::{Node, QosProfile, std_msgs};
-use crate::mission::{Mission, MissionData};
+use r2r::{Node, QosProfile, sensor_msgs, std_msgs};
+use crate::mission::{CommonMission, Mission, MissionData};
 use std::collections::VecDeque;
 use std::sync::{Arc, Mutex};
 use std::sync::atomic::{AtomicBool, Ordering};
@@ -9,7 +9,8 @@ use std::thread::{self, sleep};
 use std::time::Duration;
 use crate::ros_mission;
 
-pub type MissionVec = VecDeque<Mission>;
+pub type MissionBox = Box<dyn Mission>;
+pub type MissionVec = VecDeque<MissionBox>;
 struct MissionThreadData {
     mission_list: Arc<Mutex<MissionVec>>,
     conc_mission_list: Arc<Mutex<MissionVec>>,
@@ -31,7 +32,7 @@ impl MissionThreadData {
         }
     }
 
-    fn with_mission_list(&self, func: impl FnOnce(&mut MissionVec) -> Option<Mission>, is_concurrent: bool) -> Option<Mission> {
+    fn with_mission_list(&self, func: impl FnOnce(&mut MissionVec) -> Option<MissionBox>, is_concurrent: bool) -> Option<MissionBox> {
         let mut guard = if is_concurrent {
             self.conc_mission_list.try_lock().expect("Concurrent mission lock is poisoned!")
         } else {
@@ -40,16 +41,16 @@ impl MissionThreadData {
         func(&mut guard)
     }
 
-    pub fn pop_front(&self) -> Option<Mission> {
-        let func = move |mission_list: &mut VecDeque<Mission>| {
+    pub fn pop_front(&self) -> Option<Box<dyn Mission>> {
+        let func = move |mission_list: &mut MissionVec| {
             mission_list.pop_front()
         };
         self.with_mission_list(func, false)
     }
 
-    pub fn push_back(&self, mission : Mission) -> Option<Mission> {
-        let func = move |mission_list: &mut VecDeque<Mission>| {
-            mission_list.push_back(mission);
+    pub fn push_back(&self, mission : impl Mission + 'static) -> Option<MissionBox> {
+        let func = move |mission_list: &mut MissionVec| {
+            mission_list.push_back(Box::new(mission));
             None
         };
         self.with_mission_list(func, false)
@@ -77,18 +78,18 @@ impl MissionScheduler {
     }
 
     #[allow(unused)]
-    pub fn push_back(&self, mission: Mission) {
-        let func = move |mission_list: &mut VecDeque<Mission>| {
-            mission_list.push_back(mission);
+    pub fn push_back(&self, mission: impl Mission + 'static) {
+        let func = move |mission_list: &mut VecDeque<MissionBox>| {
+            mission_list.push_back(Box::new(mission));
             None
         };
         self.scheduler_data.with_mission_list(func, false);
     }
 
     #[allow(unused)]
-    pub fn conc_push_back(&self, mission: Mission) {
-        let func = move |mission_list: &mut VecDeque<Mission>| {
-            mission_list.push_back(mission);
+    pub fn conc_push_back(&self, mission: impl Mission + 'static) {
+        let func = move |mission_list: &mut MissionVec| {
+            mission_list.push_back(Box::new(mission));
             None
         };
         self.scheduler_data.with_mission_list(func, true);
@@ -96,7 +97,7 @@ impl MissionScheduler {
 
     #[allow(unused)]
     pub fn append(&self, mut mission_vec: MissionVec) {
-        let func = move |mission_list: &mut VecDeque<Mission>| {
+        let func = move |mission_list: &mut MissionVec| {
             mission_list.append(&mut mission_vec);
             None
         };
@@ -105,7 +106,7 @@ impl MissionScheduler {
 
     #[allow(unused)]
     pub fn conc_append(&self, mut mission_vec: MissionVec) {
-        let func = move |mission_list: &mut VecDeque<Mission>| {
+        let func = move |mission_list: &mut MissionVec| {
             mission_list.append(&mut mission_vec);
             None
         };
@@ -128,7 +129,7 @@ impl MissionScheduler {
     fn run_ros_topics(&mut self) {
         //TODO: We should not have this hardcoded
         let mut example_sub = self.node
-            .subscribe::<std_msgs::msg::String>("/spawn_mission", QosProfile::default())
+            .subscribe::<sensor_msgs::msg::Image>("/camera/image", QosProfile::default())
             .expect("Failed to create example subscriber!");
         let example_pub= self.node
             .create_publisher::<std_msgs::msg::String>("/example", QosProfile::default())
@@ -139,10 +140,8 @@ impl MissionScheduler {
             while ! scheduler_data.stop.load(Ordering::Relaxed) {
                 match example_sub.next().await {
                     Some(msg) => {
-                        if msg.data.eq("do-something") {
-                            let mission = ros_mission::new();
-                            scheduler_data.push_back(mission);
-                        }
+                        let mission = ros_mission::new(msg);
+                        scheduler_data.push_back(mission);
                     }
                     None => break,
                 }
diff --git a/src/mission-planner/src/pymission.rs b/src/mission-planner/src/pymission.rs
index e88dd4a..fd0316c 100644
--- a/src/mission-planner/src/pymission.rs
+++ b/src/mission-planner/src/pymission.rs
@@ -1,6 +1,6 @@
 use std::ffi::CStr;
 
-use crate::mission::{Mission, Task, MissionData, MissionResult};
+use crate::mission::{CommonMission, Task, MissionData, MissionResult};
 
 use pyo3::{PyResult, Python, ffi::c_str, prelude::*, types::{IntoPyDict, PyDict}};
 
@@ -62,7 +62,7 @@ impl Task for PyTask {
 
 }
 
-pub fn get_mission_from(file: &CStr, file_name: &CStr) -> Mission {
+pub fn get_mission_from(file: &CStr, file_name: &CStr) -> CommonMission {
     let pytask = c_str!(include_str!("pymission.py"));
 
     let res = Python::with_gil(|py| -> PyResult<(String, Vec<PyObject>)> {
@@ -89,7 +89,7 @@ pub fn get_mission_from(file: &CStr, file_name: &CStr) -> Mission {
         task_list.push(Box::new(task));
     }
 
-    Mission {
+    CommonMission {
         name,
         task_list
     }
diff --git a/src/mission-planner/src/ros_mission.rs b/src/mission-planner/src/ros_mission.rs
index ae32002..88b4e33 100644
--- a/src/mission-planner/src/ros_mission.rs
+++ b/src/mission-planner/src/ros_mission.rs
@@ -1,16 +1,48 @@
-use crate::mission::{Mission, Task, MissionResult, MissionData, RustTask};
+use crate::mission::{Mission, CommonMission, Task, MissionResult, MissionData, RustTask};
+use cv_bridge::{CvImage, cv_image};
+use opencv::{core::{CV_8UC1, MatTrait, ToInputArray, no_array}, highgui};
+use r2r::sensor_msgs::msg::Image;
+
+struct ExampleImageMission {
+    name: String,
+    image: Image,
+}
+
+impl Mission for ExampleImageMission {
+    fn name(&self) -> &String {
+        return &self.name
+    }
+    fn run(&self, data: &MissionData) -> MissionResult {
+        ros_example(&self.image, data)
+    }
+    
+}
+
+fn ros_example(image: &Image, data: &MissionData) -> MissionResult {
+
+    let mut cv_image = CvImage::from_imgmsg(image.clone()).expect("Failed to get cvimage!");
+    let mat = match cv_image.as_cvmat() {
+        Ok(mat) => mat,
+        Err(err) => {
+            println!("Error getting mat: {}", err);
+            return Err(false)
+        }
+    };
+
+    // let scalar =opencv::core::Scalar::new(0.0, 0.0, 0.0, 0.0);
+    // let mat = opencv::core::Mat::new_rows_cols_with_default(1024, 1024, CV_8UC1, scalar)
+    //     .unwrap();
+
+    let window = "foo";
+    highgui::named_window(window, highgui::WINDOW_AUTOSIZE).unwrap();
+    highgui::imshow(window, &mat).unwrap();
+    highgui::wait_key(1).unwrap();
 
-fn ros_example(_data: &MissionData) -> MissionResult {
-    println!("Ros mission invoked!");
     Ok(())
 }
 
-pub fn new() -> Mission {
+pub fn new(image: Image) -> impl Mission {
     let name = "ros-example-mission".to_string();
-    let task = RustTask::new("ros-example-task".to_string(), Some(ros_example), None);
-    let task_list: Vec<Box<dyn Task>> = vec![
-        Box::new(task)
-    ];
 
-    Mission { name, task_list }
+    ExampleImageMission { name, image }
 }

From 2c8035d1bba552e129d2e3614baa7f6d9bae038c Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Sun, 21 Sep 2025 08:23:38 -0400
Subject: [PATCH 06/19] mission-planner: Fix most warnings

---
 src/mission-planner/src/main.rs              |  5 ++---
 src/mission-planner/src/mission_scheduler.rs |  2 +-
 src/mission-planner/src/pymission.rs         | 18 +++++++++---------
 src/mission-planner/src/ros_mission.rs       |  8 ++++----
 4 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index 5ceedb3..7424397 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -8,14 +8,13 @@ mod ros_mission;
 
 include!(concat!(env!("OUT_DIR"), "/bindings.rs"));
 
-use std::vec;
 use std::{collections::VecDeque};
 use std::time::{Duration, Instant};
 
 
 use pyo3::{ffi::c_str};
 
-use crate::mission_scheduler::{MissionBox, MissionVec};
+use crate::mission_scheduler::MissionBox;
 use crate::{
     mission::{CommonMission}, mission_scheduler::MissionScheduler
 };
@@ -48,7 +47,7 @@ fn main() {
     let mut scheduler = MissionScheduler::start();
     scheduler.append(mission_list);
     scheduler.conc_append(conc_mission_list);
-    let data = scheduler.get_data();
+    let _data = scheduler.get_data();
 
     let start = Instant::now();
     scheduler.run();
diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index d88c71f..e7d3c57 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -1,7 +1,7 @@
 use futures::StreamExt;
 use futures::executor::ThreadPool;
 use r2r::{Node, QosProfile, sensor_msgs, std_msgs};
-use crate::mission::{CommonMission, Mission, MissionData};
+use crate::mission::{Mission, MissionData};
 use std::collections::VecDeque;
 use std::sync::{Arc, Mutex};
 use std::sync::atomic::{AtomicBool, Ordering};
diff --git a/src/mission-planner/src/pymission.rs b/src/mission-planner/src/pymission.rs
index fd0316c..4ab0a6b 100644
--- a/src/mission-planner/src/pymission.rs
+++ b/src/mission-planner/src/pymission.rs
@@ -2,17 +2,17 @@ use std::ffi::CStr;
 
 use crate::mission::{CommonMission, Task, MissionData, MissionResult};
 
-use pyo3::{PyResult, Python, ffi::c_str, prelude::*, types::{IntoPyDict, PyDict}};
+use pyo3::{PyResult, Python, ffi::c_str, prelude::*, types::PyDict};
 
 pub struct PyTask {
     name: String,
-    pytask_obj: PyObject
+    pytask_obj: Py<PyAny>
 }
 
 impl PyTask {
     //Assume pytask
-    pub fn new(pytask_obj: PyObject) -> Self {
-        let res = Python::with_gil(|py| -> PyResult<String> {
+    pub fn new(pytask_obj: Py<PyAny>) -> Self {
+        let res = Python::attach(|py| -> PyResult<String> {
             let name = pytask_obj.getattr(py, "name")?.to_string();
             Ok(name)
         });
@@ -26,7 +26,7 @@ impl PyTask {
     }
 
     fn run_with(&self, option: &str, data: &MissionData) -> MissionResult {
-        let res = Python::with_gil(|py| -> PyResult<()> {
+        let res = Python::attach(|py| -> PyResult<()> {
             let task_obj = self.pytask_obj.as_ref();
             //let dict = data.clone().into_py_dict(py)?;
             //For now let's just pass a dummy data
@@ -65,15 +65,15 @@ impl Task for PyTask {
 pub fn get_mission_from(file: &CStr, file_name: &CStr) -> CommonMission {
     let pytask = c_str!(include_str!("pymission.py"));
 
-    let res = Python::with_gil(|py| -> PyResult<(String, Vec<PyObject>)> {
+    let res = Python::attach(|py| -> PyResult<(String, Vec<Py<PyAny>>)> {
         PyModule::from_code(py, pytask, c_str!("pymission.py"), c_str!("pymission"))?;
-        let pymission: PyObject = PyModule::from_code(py, file, file_name, c_str!(""))?
+        let pymission: Py<PyAny> = PyModule::from_code(py, file, file_name, c_str!(""))?
             .getattr("new")?
             .into();
         let pymission = pymission.call0(py)?;
         let name = pymission.getattr(py, "name")?.to_string();
-        let task_list: PyObject = pymission.getattr(py, "task_list")?.into();
-        let task_list: Vec<PyObject> = task_list.extract(py)?;
+        let task_list: Py<PyAny> = pymission.getattr(py, "task_list")?.into();
+        let task_list: Vec<Py<PyAny>> = task_list.extract(py)?;
         Ok((name, task_list))
     });
 
diff --git a/src/mission-planner/src/ros_mission.rs b/src/mission-planner/src/ros_mission.rs
index 88b4e33..6ad98d8 100644
--- a/src/mission-planner/src/ros_mission.rs
+++ b/src/mission-planner/src/ros_mission.rs
@@ -1,6 +1,6 @@
-use crate::mission::{Mission, CommonMission, Task, MissionResult, MissionData, RustTask};
-use cv_bridge::{CvImage, cv_image};
-use opencv::{core::{CV_8UC1, MatTrait, ToInputArray, no_array}, highgui};
+use crate::mission::{Mission, MissionResult, MissionData};
+use cv_bridge::CvImage;
+use opencv::highgui;
 use r2r::sensor_msgs::msg::Image;
 
 struct ExampleImageMission {
@@ -18,7 +18,7 @@ impl Mission for ExampleImageMission {
     
 }
 
-fn ros_example(image: &Image, data: &MissionData) -> MissionResult {
+fn ros_example(image: &Image, _data: &MissionData) -> MissionResult {
 
     let mut cv_image = CvImage::from_imgmsg(image.clone()).expect("Failed to get cvimage!");
     let mat = match cv_image.as_cvmat() {

From e95639c44a48931144820123285710681af1b740 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Mon, 22 Sep 2025 15:39:20 -0400
Subject: [PATCH 07/19] mision-planner: Let main handle subscribing

---
 src/mission-planner/src/main.rs              | 68 +++++++++++++++++-
 src/mission-planner/src/mission_scheduler.rs | 74 ++++----------------
 2 files changed, 77 insertions(+), 65 deletions(-)

diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index 7424397..62d3ed3 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -8,18 +8,79 @@ mod ros_mission;
 
 include!(concat!(env!("OUT_DIR"), "/bindings.rs"));
 
+use std::sync::Arc;
+use std::sync::atomic::Ordering;
+use std::thread::sleep;
 use std::{collections::VecDeque};
 use std::time::{Duration, Instant};
 
-
+use futures::prelude::*;
+use futures::future::BoxFuture;
 use pyo3::{ffi::c_str};
+use r2r::{Node, QosProfile, sensor_msgs, std_msgs};
 
-use crate::mission_scheduler::MissionBox;
+use crate::mission_scheduler::{MissionBox, MissionThreadData};
 use crate::{
     mission::{CommonMission}, mission_scheduler::MissionScheduler
 };
 
 
+fn run_ros_topics(scheduler: &MissionScheduler) -> Node {
+    let ctx = r2r::Context::create().expect("Failed to create r2r context!");
+    let mut node = r2r::Node::create(ctx, "mission_planner", "namespace")
+        .expect("Failed to get Node!");
+
+    let mut example_sub = node
+        .subscribe::<sensor_msgs::msg::Image>("/camera/image", QosProfile::default())
+        .expect("Failed to create example subscriber!");
+    let example_pub= node
+        .create_publisher::<std_msgs::msg::String>("/example", QosProfile::default())
+        .expect("Failed to create example publisher!");
+
+    let example_subscriber_func = 
+    |thread_data : Arc<MissionThreadData>| {
+        let scheduler_data = thread_data.clone();    
+        let pin: BoxFuture<'static, ()> = Box::pin(async move {
+            while ! scheduler_data.stop.load(Ordering::Relaxed) {
+                match example_sub.next().await {
+                    Some(msg) => {
+                        let mission = ros_mission::new(msg);
+                        scheduler_data.push_back(mission);
+                    }
+                    None => break,
+                }
+            }
+        });
+        pin
+    };
+
+    let example_publisher_func = 
+    |thread_data : Arc<MissionThreadData>| {
+        let scheduler_data = thread_data.clone();
+        let pin: BoxFuture<'static, ()> = Box::pin(async move {
+            let mut counter = 0;
+            let mut stop = false;
+            while ! stop {
+                let msg = std_msgs::msg::String {
+                    data: format!("{}", counter),
+                };
+                example_pub.publish(&msg).expect("Failed to publish example!");
+                counter += 1;
+                stop = scheduler_data.stop.load(Ordering::Relaxed);
+                //Should we use a ros timer instead?
+                sleep(Duration::from_secs(1));
+                //This should probably go on another thread
+            }
+        });
+        pin
+    };
+
+    scheduler.add_async_thread(example_publisher_func);
+    scheduler.add_async_thread(example_subscriber_func);
+
+    node
+}
+    
 fn main() {
     let pytest = c_str!(include_str!("pymission_example.py"));
     let pymission_example = pymission::get_mission_from(pytest, c_str!("pymission_example.py"));
@@ -51,8 +112,9 @@ fn main() {
 
     let start = Instant::now();
     scheduler.run();
+    let mut node = run_ros_topics(&scheduler);
     while start.elapsed() < Duration::from_secs(15) {
-        scheduler.ros_spin();
+        node.spin_once(Duration::from_millis(100));
     }
     scheduler.stop();
 
diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index e7d3c57..29865c4 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -1,22 +1,19 @@
-use futures::StreamExt;
 use futures::executor::ThreadPool;
-use r2r::{Node, QosProfile, sensor_msgs, std_msgs};
+use futures::future::BoxFuture;
 use crate::mission::{Mission, MissionData};
 use std::collections::VecDeque;
 use std::sync::{Arc, Mutex};
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::thread::{self, sleep};
-use std::time::Duration;
-use crate::ros_mission;
 
 pub type MissionBox = Box<dyn Mission>;
 pub type MissionVec = VecDeque<MissionBox>;
-struct MissionThreadData {
-    mission_list: Arc<Mutex<MissionVec>>,
-    conc_mission_list: Arc<Mutex<MissionVec>>,
+pub struct MissionThreadData {
+    pub(super) mission_list: Arc<Mutex<MissionVec>>,
+    pub(super) conc_mission_list: Arc<Mutex<MissionVec>>,
     mission_data: Arc<MissionData>,
     run: AtomicBool,
-    stop: AtomicBool,
+    pub(crate) stop: AtomicBool,
     waiting: AtomicBool,
 }
 
@@ -61,17 +58,15 @@ impl MissionThreadData {
 pub struct MissionScheduler {
     normal_handle: thread::JoinHandle<()>,
     concurrent_handle: thread::JoinHandle<()>,
-    node: Node,
     pool: ThreadPool,
     scheduler_data : Arc<MissionThreadData>,
 }
 
 impl MissionScheduler {
-    fn new(normal_handle: thread::JoinHandle<()>, concurrent_handle: thread::JoinHandle<()>, node: Node, scheduler_data: Arc<MissionThreadData>) -> Self {
+    fn new(normal_handle: thread::JoinHandle<()>, concurrent_handle: thread::JoinHandle<()>, scheduler_data: Arc<MissionThreadData>) -> Self {
         Self {
             normal_handle,
             concurrent_handle,
-            node,
             scheduler_data,
             pool: ThreadPool::new().expect("Failed to create ThreadPool"),
         }
@@ -125,48 +120,12 @@ impl MissionScheduler {
     // pub fn concurrent_append(&mut self, mission: &mut Vec<Box<dyn Mission<'static> + Send >>) {
     //     self.concurrent_mission_list.append(mission);
     // }
+    pub fn add_async_thread<F>(&self, func: F)
+    where F : FnOnce(Arc<MissionThreadData>) -> BoxFuture<'static, ()>
+    {
+        let future = func(self.scheduler_data.clone());
+        self.pool.spawn_ok(future);
 
-    fn run_ros_topics(&mut self) {
-        //TODO: We should not have this hardcoded
-        let mut example_sub = self.node
-            .subscribe::<sensor_msgs::msg::Image>("/camera/image", QosProfile::default())
-            .expect("Failed to create example subscriber!");
-        let example_pub= self.node
-            .create_publisher::<std_msgs::msg::String>("/example", QosProfile::default())
-            .expect("Failed to create example publisher!");
-
-        let scheduler_data = self.scheduler_data.clone();
-        let example_subscriber_func = async move {
-            while ! scheduler_data.stop.load(Ordering::Relaxed) {
-                match example_sub.next().await {
-                    Some(msg) => {
-                        let mission = ros_mission::new(msg);
-                        scheduler_data.push_back(mission);
-                    }
-                    None => break,
-                }
-            }
-        };
-
-        let scheduler_data = self.scheduler_data.clone();
-        let example_publisher_func = async move {
-            let mut counter = 0;
-            let mut stop = false;
-            while ! stop {
-                let msg = std_msgs::msg::String {
-                    data: format!("{}", counter),
-                };
-                example_pub.publish(&msg).expect("Failed to publish example!");
-                counter += 1;
-                stop = scheduler_data.stop.load(Ordering::Relaxed);
-                //Should we use a ros timer instead?
-                sleep(Duration::from_secs(1));
-                //This should probably go on another thread
-            }
-        };
-    
-        self.pool.spawn_ok(example_publisher_func);
-        self.pool.spawn_ok(example_subscriber_func);
     }
    
     pub fn start() -> Self {
@@ -255,15 +214,10 @@ impl MissionScheduler {
         
         let normal_handle = thread::spawn(normal_func);
         let conc_handle = thread::spawn(concurrent_func);
-        let ctx = r2r::Context::create().expect("Failed to create r2r context!");
-        let node = r2r::Node::create(ctx, "mission_scheduler", "namespace")
-            .expect("Failed to get Node!");
-        MissionScheduler::new(normal_handle, conc_handle, node, scheduler_data_orig)
+        MissionScheduler::new(normal_handle, conc_handle, scheduler_data_orig)
     }
 
     pub fn run(&mut self) {
-        self.run_ros_topics();
-
         self.scheduler_data.run.store(true, Ordering::Relaxed);
     }
 
@@ -272,8 +226,4 @@ impl MissionScheduler {
         self.normal_handle.join().expect("Failed to join mission handle!");
         self.concurrent_handle.join().expect("Failed to join concurrent mission handle!");
     }
-
-    pub fn ros_spin(&mut self) {
-        self.node.spin_once(Duration::from_millis(100));
-    }
 }

From a2eba5477488748d8251011c11184235501cfd59 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Mon, 22 Sep 2025 16:16:46 -0400
Subject: [PATCH 08/19] Add submodules (for real this time)

---
 third_party/cv-bridge-rs              | 1 +
 third_party/image_transport_tutorials | 1 +
 2 files changed, 2 insertions(+)
 create mode 160000 third_party/cv-bridge-rs
 create mode 160000 third_party/image_transport_tutorials

diff --git a/third_party/cv-bridge-rs b/third_party/cv-bridge-rs
new file mode 160000
index 0000000..c47ce86
--- /dev/null
+++ b/third_party/cv-bridge-rs
@@ -0,0 +1 @@
+Subproject commit c47ce8695e4e1172fc146c4817f2ea43579fb202
diff --git a/third_party/image_transport_tutorials b/third_party/image_transport_tutorials
new file mode 160000
index 0000000..99af171
--- /dev/null
+++ b/third_party/image_transport_tutorials
@@ -0,0 +1 @@
+Subproject commit 99af171fe90b804c8b46f2f888e7c48d505af9df

From 02958f13ac044f64dbe1dced4774b3dd55d84608 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Mon, 22 Sep 2025 16:52:49 -0400
Subject: [PATCH 09/19] mission-planner: Remove some unnecesary muts

---
 src/mission-planner/src/main.rs              | 2 +-
 src/mission-planner/src/mission_scheduler.rs | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index 62d3ed3..c012f78 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -105,7 +105,7 @@ fn main() {
     ];
     let conc_mission_list = VecDeque::from(conc_mission_list);
 
-    let mut scheduler = MissionScheduler::start();
+    let scheduler = MissionScheduler::start();
     scheduler.append(mission_list);
     scheduler.conc_append(conc_mission_list);
     let _data = scheduler.get_data();
diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index 29865c4..a1f959c 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -217,7 +217,7 @@ impl MissionScheduler {
         MissionScheduler::new(normal_handle, conc_handle, scheduler_data_orig)
     }
 
-    pub fn run(&mut self) {
+    pub fn run(&self) {
         self.scheduler_data.run.store(true, Ordering::Relaxed);
     }
 

From bf8336549ccea0e0119d472662a9a5b8addaeb59 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Mon, 22 Sep 2025 17:10:20 -0400
Subject: [PATCH 10/19] mission-planner: Remove unnecesary Arcs

---
 src/mission-planner/src/mission_scheduler.rs | 22 ++++++++++----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index a1f959c..afad9dc 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -9,20 +9,20 @@ use std::thread::{self, sleep};
 pub type MissionBox = Box<dyn Mission>;
 pub type MissionVec = VecDeque<MissionBox>;
 pub struct MissionThreadData {
-    pub(super) mission_list: Arc<Mutex<MissionVec>>,
-    pub(super) conc_mission_list: Arc<Mutex<MissionVec>>,
-    mission_data: Arc<MissionData>,
+    mission_list: Mutex<MissionVec>,
+    conc_mission_list: Mutex<MissionVec>,
+    mission_data: MissionData,
     run: AtomicBool,
-    pub(crate) stop: AtomicBool,
+    pub stop: AtomicBool,
     waiting: AtomicBool,
 }
 
 impl MissionThreadData {
     fn new() -> Self {
         Self {
-            mission_list: Arc::new(Mutex::new(VecDeque::new())),
-            conc_mission_list: Arc::new(Mutex::new(VecDeque::new())),
-            mission_data: Arc::new(MissionData::new()),
+            mission_list: Mutex::new(VecDeque::new()),
+            conc_mission_list: Mutex::new(VecDeque::new()),
+            mission_data: MissionData::new(),
             run: AtomicBool::new(false),
             stop: AtomicBool::new(false),
             waiting: AtomicBool::new(false),
@@ -108,8 +108,9 @@ impl MissionScheduler {
         self.scheduler_data.with_mission_list(func, true);
     }
 
-    pub fn get_data(&self) -> Arc<MissionData> {
-        self.scheduler_data.mission_data.clone()
+    #[allow(unused)]
+    pub fn get_data(&self) -> &MissionData {
+        &self.scheduler_data.mission_data
     }
 
     #[allow(unused)]
@@ -177,7 +178,6 @@ impl MissionScheduler {
         let scheduler_data = scheduler_data_orig.clone();
         let concurrent_func = move || {
             let mut stop = false;
-            let conc_mission_list = scheduler_data.conc_mission_list.clone();
             let data = &scheduler_data.mission_data;
             while ! stop {
                 let run = scheduler_data.run.load(Ordering::Relaxed);
@@ -187,7 +187,7 @@ impl MissionScheduler {
                     continue
                 }
 
-                let getter = conc_mission_list
+                let getter = scheduler_data.conc_mission_list
                     .try_lock()
                     .expect("Concurrent mission lock is poisoned!");
                 for mission in &*getter {

From 8275d3359f9bd598e60051d80c5133046dc8fa7b Mon Sep 17 00:00:00 2001
From: JuanDelPueblo <edyancruz@outlook.com>
Date: Sat, 26 Jul 2025 11:28:41 -0700
Subject: [PATCH 11/19] Fix stonefish library not being recognized by
 intellisense

---
 .vscode/c_cpp_properties.json | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)
 create mode 100644 .vscode/c_cpp_properties.json

diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..97c74f9
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,17 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/usr/local/include/Stonefish/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-x64"
+        }
+    ],
+    "version": 4
+}
\ No newline at end of file

From 7b6a14c8ab9526b04a4d966700c8a954a4cb2b27 Mon Sep 17 00:00:00 2001
From: JuanDelPueblo <edyancruz@outlook.com>
Date: Thu, 28 Aug 2025 16:02:14 -0400
Subject: [PATCH 12/19] Add Docker image for simulation

---
 .vscode/settings.json                   | 53 +++++++++++++------------
 docker/simulation/simulation.Dockerfile | 34 ++++++++++++++++
 docker/simulation/simulation.yaml       | 23 +++++++++++
 3 files changed, 84 insertions(+), 26 deletions(-)
 create mode 100644 docker/simulation/simulation.Dockerfile
 create mode 100644 docker/simulation/simulation.yaml

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 5406887..f9b311b 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,27 +1,28 @@
 {
-        "python.autoComplete.extraPaths": [
-            "/home/catkin_ws/src/hydrus-software-stack/scripts",
-            "/catkin_ws/devel/lib/python3/dist-packages",
-            "/opt/ros/noetic/lib/python3/dist-packages"
-        ],
-        "python.analysis.extraPaths": [
-            "/home/catkin_ws/src/hydrus-software-stack/scripts",
-            "/catkin_ws/devel/lib/python3/dist-packages",
-            "/opt/ros/noetic/lib/python3/dist-packages"
-        ],
-        "pyrefly.trace.server": "verbose",
-        "editor.hover.enabled": true,
-        "editor.snippetSuggestions": "inline",
-        "python.testing.unittestArgs": [
-            "-v",
-            "-s",
-            "./test",
-            "-p",
-            "*_test.py"
-        ],
-        "python.testing.pytestEnabled": false,
-        "python.testing.unittestEnabled": true,
-        "files.associations": {
-            "*.hss": "yaml"
-        }
-}
+    "python.autoComplete.extraPaths": [
+        "/home/catkin_ws/src/hydrus-software-stack/scripts",
+        "/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/catkin_ws/src/hydrus-software-stack/scripts",
+        "/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "pyrefly.trace.server": "verbose",
+    "editor.hover.enabled": true,
+    "editor.snippetSuggestions": "inline",
+    "python.testing.unittestArgs": [
+        "-v",
+        "-s",
+        "./test",
+        "-p",
+        "*_test.py"
+    ],
+    "python.testing.pytestEnabled": false,
+    "python.testing.unittestEnabled": true,
+    "files.associations": {
+        "*.hss": "yaml"
+    },
+    "cmake.ignoreCMakeListsMissing": true
+}
\ No newline at end of file
diff --git a/docker/simulation/simulation.Dockerfile b/docker/simulation/simulation.Dockerfile
new file mode 100644
index 0000000..d06f05d
--- /dev/null
+++ b/docker/simulation/simulation.Dockerfile
@@ -0,0 +1,34 @@
+FROM osrf/ros:humble-desktop-full-jammy
+RUN apt-get update
+
+ARG DEBIAN_FRONTEND=noninteractive
+
+RUN apt-get upgrade -y && \
+	apt-get install -y \
+	libglm-dev \
+	libfreetype6-dev \
+	libsdl2-dev \
+	python3-colcon-common-extensions \
+	python3-rosdep \
+	python3-vcstool
+
+# ROS 2 workspace setup
+SHELL ["/bin/bash", "-c"]
+
+ENV ROS_WS=/home/ros2_ws
+
+RUN mkdir -p /${ROS_WS}/src \
+	&& echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc 
+
+WORKDIR $ROS_WS/src
+
+RUN cd /${ROS_WS}/ \
+	&& colcon build --symlink-install \
+	&& source install/setup.bash
+
+WORKDIR /home/
+
+RUN git clone https://github.com/patrykcieslak/stonefish
+RUN cd stonefish && mkdir build  && cd build  && cmake ..  && make -j$(nproc) && sudo make install
+
+WORKDIR $ROS_WS/src
diff --git a/docker/simulation/simulation.yaml b/docker/simulation/simulation.yaml
new file mode 100644
index 0000000..b6e46e1
--- /dev/null
+++ b/docker/simulation/simulation.yaml
@@ -0,0 +1,23 @@
+services:
+  simulation:
+    build:
+      context: ../../../hydrus-software-stack
+      dockerfile: docker/simulation/simulation.Dockerfile
+    privileged: true
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: all
+              capabilities: [gpu]
+    environment:
+      - DISPLAY=${DISPLAY}
+    ulimits:
+      nofile:
+        soft: 1024
+        hard: 524288
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix
+    stdin_open: true
+    tty: true

From dc392eee3a377ff69abb2a4646060f6078b912b2 Mon Sep 17 00:00:00 2001
From: JuanDelPueblo <edyancruz@outlook.com>
Date: Thu, 28 Aug 2025 16:23:51 -0400
Subject: [PATCH 13/19] Add ros 2 stonefish package to git

---
 docker/simulation/simulation.Dockerfile |  8 +++++---
 simulation/launch/hydrussim.launch.py   | 27 +++++++++++++++++++++++++
 2 files changed, 32 insertions(+), 3 deletions(-)
 create mode 100644 simulation/launch/hydrussim.launch.py

diff --git a/docker/simulation/simulation.Dockerfile b/docker/simulation/simulation.Dockerfile
index d06f05d..6f1910b 100644
--- a/docker/simulation/simulation.Dockerfile
+++ b/docker/simulation/simulation.Dockerfile
@@ -20,10 +20,12 @@ ENV ROS_WS=/home/ros2_ws
 RUN mkdir -p /${ROS_WS}/src \
 	&& echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc 
 
-WORKDIR $ROS_WS/src
+WORKDIR $ROS_WS/
+
+RUN cd src \
+	&& git clone https://github.com/patrykcieslak/stonefish_ros2.git
 
-RUN cd /${ROS_WS}/ \
-	&& colcon build --symlink-install \
+RUN colcon build --symlink-install \
 	&& source install/setup.bash
 
 WORKDIR /home/
diff --git a/simulation/launch/hydrussim.launch.py b/simulation/launch/hydrussim.launch.py
new file mode 100644
index 0000000..3163bbb
--- /dev/null
+++ b/simulation/launch/hydrussim.launch.py
@@ -0,0 +1,27 @@
+from launch_ros.substitutions import FindPackageShare
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                PathJoinSubstitution([
+                    FindPackageShare('stonefish_ros2'),
+                    'launch',
+                    'stonefish_simulator.launch.py'
+                ])
+            ]),
+            launch_arguments={
+                'simulation_data': PathJoinSubstitution([FindPackageShare('my_package'), 'data']),
+                'scenario_desc': PathJoinSubstitution([FindPackageShare('my_package'), 'scenarios', 'simulation.scn']),
+                'simulation_rate': '300.0',
+                'window_res_x': '1200',
+                'window_res_y': '800',
+                'rendering_quality': 'high'
+            }.items()
+        )
+    ])

From 7be39dd387b8d895ee5f0836b8b9018fb2548d34 Mon Sep 17 00:00:00 2001
From: JuanDelPueblo <edyancruz@outlook.com>
Date: Tue, 2 Sep 2025 10:53:57 -0400
Subject: [PATCH 14/19] Add README.md with instructions for running the
 simulation

---
 simulation/README.md | 13 +++++++++++++
 1 file changed, 13 insertions(+)
 create mode 100644 simulation/README.md

diff --git a/simulation/README.md b/simulation/README.md
new file mode 100644
index 0000000..9dd4199
--- /dev/null
+++ b/simulation/README.md
@@ -0,0 +1,13 @@
+# How to run:
+
+Docker is required to run the simulation along with a Linux install (WSL works but very slow). An Nvidia GPU is also highly recommended.
+
+1. Go to docker/simulation folder
+
+2. Run `docker compose -f simulation.yaml run simulation`
+
+3. Run `cd /home/ros2_ws`
+
+4. Run `colcon build --symlink-install`
+
+5. Run `ros2 launch hydrus_sim_ros2 hydrussim.launch.py`
\ No newline at end of file

From ade2b59eb5cc7d2e6189a141fd8b4846ebc1f8ed Mon Sep 17 00:00:00 2001
From: JuanDelPueblo <edyancruz@outlook.com>
Date: Thu, 18 Sep 2025 09:38:22 -0400
Subject: [PATCH 15/19] Simplify docker compose file

---
 docker/simulation/simulation.yaml | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/docker/simulation/simulation.yaml b/docker/simulation/simulation.yaml
index b6e46e1..ebf846c 100644
--- a/docker/simulation/simulation.yaml
+++ b/docker/simulation/simulation.yaml
@@ -4,15 +4,10 @@ services:
       context: ../../../hydrus-software-stack
       dockerfile: docker/simulation/simulation.Dockerfile
     privileged: true
-    deploy:
-      resources:
-        reservations:
-          devices:
-            - driver: nvidia
-              count: all
-              capabilities: [gpu]
+    runtime: nvidia
     environment:
       - DISPLAY=${DISPLAY}
+      - PYTHONUNBUFFERED=1
     ulimits:
       nofile:
         soft: 1024

From 35231d6c05677e63e19bf9a1b61eaa4d07fb0597 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Mon, 20 Oct 2025 16:44:50 -0400
Subject: [PATCH 16/19] mission-planner: Move tests to unit test

---
 Cargo.lock                      | 199 +++++++++++++++-----------------
 Cargo.toml                      |   5 +-
 src/mission-planner/Cargo.toml  |   2 +-
 src/mission-planner/src/main.rs |  88 ++++++++------
 4 files changed, 151 insertions(+), 143 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 889d005..911cfc2 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -13,9 +13,9 @@ dependencies = [
 
 [[package]]
 name = "anstream"
-version = "0.6.20"
+version = "0.6.21"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3ae563653d1938f79b1ab1b5e668c87c76a9930414574a6583a7b7e11a8e6192"
+checksum = "43d5b281e737544384e969a5ccad3f1cdd24b48086a0fc1b2a5262a26b8f4f4a"
 dependencies = [
  "anstyle",
  "anstyle-parse",
@@ -28,9 +28,9 @@ dependencies = [
 
 [[package]]
 name = "anstyle"
-version = "1.0.11"
+version = "1.0.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "862ed96ca487e809f1c8e5a8447f6ee2cf102f846893800b20cebdf541fc6bbd"
+checksum = "5192cca8006f1fd4f7237516f40fa183bb07f8fbdfedaa0036de5ea9b0b45e78"
 
 [[package]]
 name = "anstyle-parse"
@@ -109,9 +109,9 @@ dependencies = [
 
 [[package]]
 name = "bitflags"
-version = "2.9.4"
+version = "2.10.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2261d10cca569e4643e526d8dc2e62e433cc8aba21ab764233731f8d369bf394"
+checksum = "812e12b5285cc515a9c72a5c1d3b6d46a19dac5acfef5265968c166106e31dd3"
 
 [[package]]
 name = "block-buffer"
@@ -155,9 +155,9 @@ dependencies = [
 
 [[package]]
 name = "cc"
-version = "1.2.38"
+version = "1.2.41"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "80f41ae168f955c12fb8960b057d70d0ca153fb83182b57d86380443527be7e9"
+checksum = "ac9fe6cdbb24b6ade63616c0a0688e45bb56732262c158df3c0c4bea4ca47cb7"
 dependencies = [
  "find-msvc-tools",
  "jobserver",
@@ -176,9 +176,9 @@ dependencies = [
 
 [[package]]
 name = "cfg-if"
-version = "1.0.3"
+version = "1.0.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2fd1289c04a9ea8cb22300a459a72a385d7c73d3259e2ed7dcb2af674838cfa9"
+checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801"
 
 [[package]]
 name = "clang"
@@ -203,18 +203,18 @@ dependencies = [
 
 [[package]]
 name = "clap"
-version = "4.5.48"
+version = "4.5.50"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e2134bb3ea021b78629caa971416385309e0131b351b25e01dc16fb54e1b5fae"
+checksum = "0c2cfd7bf8a6017ddaa4e32ffe7403d547790db06bd171c1c53926faab501623"
 dependencies = [
  "clap_builder",
 ]
 
 [[package]]
 name = "clap_builder"
-version = "4.5.48"
+version = "4.5.50"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c2ba64afa3c0a6df7fa517765e31314e983f51dda798ffba27b988194fb65dc9"
+checksum = "0a4c05b9e80c5ccd3a7ef080ad7b6ba7d6fc00a985b8b157197075677c82c7a0"
 dependencies = [
  "anstream",
  "anstyle",
@@ -224,9 +224,9 @@ dependencies = [
 
 [[package]]
 name = "clap_lex"
-version = "0.7.5"
+version = "0.7.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b94f61472cee1439c0b966b47e3aca9ae07e45d070759512cd390ea2bebc6675"
+checksum = "a1d728cc89cf3aee9ff92b05e62b19ee65a02b5702cff7d5a377e32c6ae29d8d"
 
 [[package]]
 name = "colorchoice"
@@ -322,7 +322,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb"
 dependencies = [
  "libc",
- "windows-sys 0.61.0",
+ "windows-sys 0.61.2",
 ]
 
 [[package]]
@@ -333,9 +333,9 @@ checksum = "37909eebbb50d72f9059c3b6d82c0463f2ff062c9e95845c43a6c9c0355411be"
 
 [[package]]
 name = "find-msvc-tools"
-version = "0.1.2"
+version = "0.1.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1ced73b1dacfc750a6db6c0a0c3a3853c8b41997e2e2c563dc90804ae6867959"
+checksum = "52051878f80a721bb68ebfbc930e07b65ba72f2da88968ea5c06fd6ca3d3a127"
 
 [[package]]
 name = "force-send-sync"
@@ -435,9 +435,9 @@ dependencies = [
 
 [[package]]
 name = "generic-array"
-version = "0.14.7"
+version = "0.14.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a"
+checksum = "4bb6743198531e02858aeaea5398fcc883e71851fcbcb5a2f773e2fb6cb1edf2"
 dependencies = [
  "typenum",
  "version_check",
@@ -445,14 +445,14 @@ dependencies = [
 
 [[package]]
 name = "getrandom"
-version = "0.3.3"
+version = "0.3.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "26145e563e54f2cadc477553f1ec5ee650b00862f0a58bcd12cbdc5f0ea2d2f4"
+checksum = "899def5c37c4fd7b2664648c28120ecec138e4d395b459e5ca34f9cce2dd77fd"
 dependencies = [
  "cfg-if",
  "libc",
  "r-efi",
- "wasi",
+ "wasip2",
 ]
 
 [[package]]
@@ -481,9 +481,9 @@ checksum = "fc0fef456e4baa96da950455cd02c081ca953b141298e41db3fc7e36b1da849c"
 
 [[package]]
 name = "indexmap"
-version = "2.11.4"
+version = "2.12.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4b0f83760fb341a774ed326568e19f5a863af4a952def8c39f9ab92fd95b88e5"
+checksum = "6717a8d2a5a929a1a2eb43a12812498ed141a0bcfb7e8f7844fbdbe4303bba9f"
 dependencies = [
  "equivalent",
  "hashbrown",
@@ -537,9 +537,9 @@ dependencies = [
 
 [[package]]
 name = "js-sys"
-version = "0.3.80"
+version = "0.3.81"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "852f13bec5eba4ba9afbeb93fd7c13fe56147f055939ae21c43a29a0ecb2702e"
+checksum = "ec48937a97411dcb524a265206ccd4c90bb711fca92b2792c407f268825b9305"
 dependencies = [
  "once_cell",
  "wasm-bindgen",
@@ -553,18 +553,18 @@ checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe"
 
 [[package]]
 name = "libc"
-version = "0.2.175"
+version = "0.2.177"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6a82ae493e598baaea5209805c49bbf2ea7de956d50d7da0da1164f9c6d28543"
+checksum = "2874a2af47a2325c2001a6e6fad9b16a53b802102b528163885171cf92b15976"
 
 [[package]]
 name = "libloading"
-version = "0.8.8"
+version = "0.8.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "07033963ba89ebaf1584d767badaa2e8fcec21aedea6b8c0346d487d49c28667"
+checksum = "d7c4b02199fee7c5d21a5ae7d8cfa79a6ef5bb2fc834d6e9058e89c825efdc55"
 dependencies = [
  "cfg-if",
- "windows-targets",
+ "windows-link 0.2.1",
 ]
 
 [[package]]
@@ -581,9 +581,9 @@ checksum = "34080505efa8e45a4b816c349525ebe327ceaa8559756f0356cba97ef3bf7432"
 
 [[package]]
 name = "memchr"
-version = "2.7.5"
+version = "2.7.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "32a282da65faaf38286cf3be983213fcf1d2e2a58700e808f83f4ea9a4804bc0"
+checksum = "f52b00d39961fc5b2736ea853c9cc86238e165017a493d1d5c8eac6bdc4cc273"
 
 [[package]]
 name = "memoffset"
@@ -859,9 +859,9 @@ dependencies = [
 
 [[package]]
 name = "quote"
-version = "1.0.40"
+version = "1.0.41"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d"
+checksum = "ce25767e7b499d1b604768e7cde645d14cc8584231ea6b295e9c9eb22c02e1d1"
 dependencies = [
  "proc-macro2",
 ]
@@ -1002,9 +1002,9 @@ dependencies = [
 
 [[package]]
 name = "regex"
-version = "1.11.2"
+version = "1.12.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "23d7fd106d8c02486a8d64e778353d1cffe08ce79ac2e82f540c86d0facf6912"
+checksum = "843bc0191f75f3e22651ae5f1e72939ab2f72a4bc30fa80a066bd66edefc24d4"
 dependencies = [
  "aho-corasick",
  "memchr",
@@ -1014,9 +1014,9 @@ dependencies = [
 
 [[package]]
 name = "regex-automata"
-version = "0.4.10"
+version = "0.4.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6b9458fa0bfeeac22b5ca447c63aaf45f28439a709ccd244698632f9aa6394d6"
+checksum = "5276caf25ac86c8d810222b3dbb938e512c55c6831a10f3e6ed1c93b84041f1c"
 dependencies = [
  "aho-corasick",
  "memchr",
@@ -1025,9 +1025,9 @@ dependencies = [
 
 [[package]]
 name = "regex-syntax"
-version = "0.8.6"
+version = "0.8.8"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "caf4aa5b0f434c91fe5c7f1ecb6a5ece2130b02ad2a590589dda5146df959001"
+checksum = "7a2d987857b319362043e95f5353c0535c1f58eec5336fdfcf626430af7def58"
 
 [[package]]
 name = "rustc-hash"
@@ -1045,7 +1045,7 @@ dependencies = [
  "errno",
  "libc",
  "linux-raw-sys",
- "windows-sys 0.61.0",
+ "windows-sys 0.61.2",
 ]
 
 [[package]]
@@ -1068,9 +1068,9 @@ checksum = "d767eb0aabc880b29956c35734170f26ed551a859dbd361d140cdbeca61ab1e2"
 
 [[package]]
 name = "serde"
-version = "1.0.225"
+version = "1.0.228"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fd6c24dee235d0da097043389623fb913daddf92c76e9f5a1db88607a0bcbd1d"
+checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e"
 dependencies = [
  "serde_core",
  "serde_derive",
@@ -1078,18 +1078,18 @@ dependencies = [
 
 [[package]]
 name = "serde_core"
-version = "1.0.225"
+version = "1.0.228"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "659356f9a0cb1e529b24c01e43ad2bdf520ec4ceaf83047b83ddcc2251f96383"
+checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad"
 dependencies = [
  "serde_derive",
 ]
 
 [[package]]
 name = "serde_derive"
-version = "1.0.225"
+version = "1.0.228"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0ea936adf78b1f766949a4977b91d2f5595825bd6ec079aa9543ad2685fc4516"
+checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -1155,9 +1155,9 @@ checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f"
 
 [[package]]
 name = "syn"
-version = "2.0.106"
+version = "2.0.107"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ede7c438028d4436d71104916910f5bb611972c5cfd7f89b8300a8186e6fada6"
+checksum = "2a26dbd934e5451d21ef060c018dae56fc073894c5a7896f882928a76e6d081b"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -1172,15 +1172,15 @@ checksum = "df7f62577c25e07834649fc3b39fafdc597c0a3527dc1c60129201ccfcbaa50c"
 
 [[package]]
 name = "tempfile"
-version = "3.22.0"
+version = "3.23.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "84fa4d11fadde498443cca10fd3ac23c951f0dc59e080e9f4b93d4df4e4eea53"
+checksum = "2d31c77bdf42a745371d260a26ca7163f1e0924b64afa0b688e61b5a9fa02f16"
 dependencies = [
  "fastrand",
  "getrandom",
  "once_cell",
  "rustix",
- "windows-sys 0.61.0",
+ "windows-sys 0.61.2",
 ]
 
 [[package]]
@@ -1246,9 +1246,9 @@ checksum = "5d99f8c9a7727884afe522e9bd5edbfc91a3312b36a77b5fb8926e4c31a41801"
 
 [[package]]
 name = "typenum"
-version = "1.18.0"
+version = "1.19.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f"
+checksum = "562d481066bde0658276a35467c4af00bdc6ee726305698a55b86e61d7ad82bb"
 
 [[package]]
 name = "unicode-ident"
@@ -1292,15 +1292,6 @@ version = "0.9.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a"
 
-[[package]]
-name = "wasi"
-version = "0.14.7+wasi-0.2.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "883478de20367e224c0090af9cf5f9fa85bed63a95c1abf3afc5c083ebc06e8c"
-dependencies = [
- "wasip2",
-]
-
 [[package]]
 name = "wasip2"
 version = "1.0.1+wasi-0.2.4"
@@ -1312,9 +1303,9 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen"
-version = "0.2.103"
+version = "0.2.104"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ab10a69fbd0a177f5f649ad4d8d3305499c42bab9aef2f7ff592d0ec8f833819"
+checksum = "c1da10c01ae9f1ae40cbfac0bac3b1e724b320abfcf52229f80b547c0d250e2d"
 dependencies = [
  "cfg-if",
  "once_cell",
@@ -1325,9 +1316,9 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen-backend"
-version = "0.2.103"
+version = "0.2.104"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0bb702423545a6007bbc368fde243ba47ca275e549c8a28617f56f6ba53b1d1c"
+checksum = "671c9a5a66f49d8a47345ab942e2cb93c7d1d0339065d4f8139c486121b43b19"
 dependencies = [
  "bumpalo",
  "log",
@@ -1339,9 +1330,9 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen-macro"
-version = "0.2.103"
+version = "0.2.104"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fc65f4f411d91494355917b605e1480033152658d71f722a90647f56a70c88a0"
+checksum = "7ca60477e4c59f5f2986c50191cd972e3a50d8a95603bc9434501cf156a9a119"
 dependencies = [
  "quote",
  "wasm-bindgen-macro-support",
@@ -1349,9 +1340,9 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen-macro-support"
-version = "0.2.103"
+version = "0.2.104"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ffc003a991398a8ee604a401e194b6b3a39677b3173d6e74495eb51b82e99a32"
+checksum = "9f07d2f20d4da7b26400c9f4a0511e6e0345b040694e8a75bd41d578fa4421d7"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -1362,18 +1353,18 @@ dependencies = [
 
 [[package]]
 name = "wasm-bindgen-shared"
-version = "0.2.103"
+version = "0.2.104"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "293c37f4efa430ca14db3721dfbe48d8c33308096bd44d80ebaa775ab71ba1cf"
+checksum = "bad67dc8b2a1a6e5448428adec4c3e84c43e561d8c9ee8a9e5aabeb193ec41d1"
 dependencies = [
  "unicode-ident",
 ]
 
 [[package]]
 name = "widestring"
-version = "1.2.0"
+version = "1.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dd7cf3379ca1aac9eea11fba24fd7e315d621f8dfe35c8d7d2be8b793726e07d"
+checksum = "72069c3113ab32ab29e5584db3c6ec55d416895e60715417b5b883a357c3e471"
 
 [[package]]
 name = "windows"
@@ -1411,9 +1402,9 @@ dependencies = [
 
 [[package]]
 name = "windows-interface"
-version = "0.59.1"
+version = "0.59.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bd9211b69f8dcdfa817bfd14bf1c97c9188afa36f4750130fcdf3f400eca9fa8"
+checksum = "3f316c4a2570ba26bbec722032c4099d8c8bc095efccdc15688708623367e358"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -1428,9 +1419,9 @@ checksum = "5e6ad25900d524eaabdbbb96d20b4311e1e7ae1699af4fb28c17ae66c80d798a"
 
 [[package]]
 name = "windows-link"
-version = "0.2.0"
+version = "0.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "45e46c0661abb7180e7b9c281db115305d49ca1709ab8242adf09666d2173c65"
+checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5"
 
 [[package]]
 name = "windows-result"
@@ -1461,20 +1452,20 @@ dependencies = [
 
 [[package]]
 name = "windows-sys"
-version = "0.61.0"
+version = "0.61.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e201184e40b2ede64bc2ea34968b28e33622acdbbf37104f0e4a33f7abe657aa"
+checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc"
 dependencies = [
- "windows-link 0.2.0",
+ "windows-link 0.2.1",
 ]
 
 [[package]]
 name = "windows-targets"
-version = "0.53.3"
+version = "0.53.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d5fe6031c4041849d7c496a8ded650796e7b6ecc19df1a431c1a363342e5dc91"
+checksum = "4945f9f551b88e0d65f3db0bc25c33b8acea4d9e41163edf90dcd0b19f9069f3"
 dependencies = [
- "windows-link 0.1.3",
+ "windows-link 0.2.1",
  "windows_aarch64_gnullvm",
  "windows_aarch64_msvc",
  "windows_i686_gnu",
@@ -1487,51 +1478,51 @@ dependencies = [
 
 [[package]]
 name = "windows_aarch64_gnullvm"
-version = "0.53.0"
+version = "0.53.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "86b8d5f90ddd19cb4a147a5fa63ca848db3df085e25fee3cc10b39b6eebae764"
+checksum = "a9d8416fa8b42f5c947f8482c43e7d89e73a173cead56d044f6a56104a6d1b53"
 
 [[package]]
 name = "windows_aarch64_msvc"
-version = "0.53.0"
+version = "0.53.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c7651a1f62a11b8cbd5e0d42526e55f2c99886c77e007179efff86c2b137e66c"
+checksum = "b9d782e804c2f632e395708e99a94275910eb9100b2114651e04744e9b125006"
 
 [[package]]
 name = "windows_i686_gnu"
-version = "0.53.0"
+version = "0.53.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1dc67659d35f387f5f6c479dc4e28f1d4bb90ddd1a5d3da2e5d97b42d6272c3"
+checksum = "960e6da069d81e09becb0ca57a65220ddff016ff2d6af6a223cf372a506593a3"
 
 [[package]]
 name = "windows_i686_gnullvm"
-version = "0.53.0"
+version = "0.53.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9ce6ccbdedbf6d6354471319e781c0dfef054c81fbc7cf83f338a4296c0cae11"
+checksum = "fa7359d10048f68ab8b09fa71c3daccfb0e9b559aed648a8f95469c27057180c"
 
 [[package]]
 name = "windows_i686_msvc"
-version = "0.53.0"
+version = "0.53.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "581fee95406bb13382d2f65cd4a908ca7b1e4c2f1917f143ba16efe98a589b5d"
+checksum = "1e7ac75179f18232fe9c285163565a57ef8d3c89254a30685b57d83a38d326c2"
 
 [[package]]
 name = "windows_x86_64_gnu"
-version = "0.53.0"
+version = "0.53.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2e55b5ac9ea33f2fc1716d1742db15574fd6fc8dadc51caab1c16a3d3b4190ba"
+checksum = "9c3842cdd74a865a8066ab39c8a7a473c0778a3f29370b5fd6b4b9aa7df4a499"
 
 [[package]]
 name = "windows_x86_64_gnullvm"
-version = "0.53.0"
+version = "0.53.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0a6e035dd0599267ce1ee132e51c27dd29437f63325753051e71dd9e42406c57"
+checksum = "0ffa179e2d07eee8ad8f57493436566c7cc30ac536a3379fdf008f47f6bb7ae1"
 
 [[package]]
 name = "windows_x86_64_msvc"
-version = "0.53.0"
+version = "0.53.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "271414315aff87387382ec3d271b52d7ae78726f5d44ac98b4f4030c91880486"
+checksum = "d6bbff5f0aada427a1e5a6da5f1f98158182f26556f345ac9e04d36d0ebed650"
 
 [[package]]
 name = "winnow"
diff --git a/Cargo.toml b/Cargo.toml
index dfec0e3..29b2908 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,6 +1,9 @@
 [workspace]
 resolver = "3"
 
+# members = [
+#     "src/mission-planner",
+#     "third_party/cv-bridge-rs"]
 members = [
     "src/mission-planner",
-    "third_party/cv-bridge-rs"]
+    ]
diff --git a/src/mission-planner/Cargo.toml b/src/mission-planner/Cargo.toml
index b9f2dff..ae27759 100644
--- a/src/mission-planner/Cargo.toml
+++ b/src/mission-planner/Cargo.toml
@@ -4,7 +4,7 @@ version = "0.1.0"
 edition = "2024"
 
 [dependencies]
-cv-bridge = { version = "0.3.4", path = "../../third_party/cv-bridge-rs" }
+cv-bridge = { version = "0.3.4", path = "../../third_party/cv-bridge-rs", optional = true }
 futures = { version = "0.3.31", features = ["thread-pool"] }
 opencv = { version = "0.95.1", features = ["clang-runtime"] }
 r2r = "0.9.5"
diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index c012f78..51f6ce7 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -24,12 +24,16 @@ use crate::{
     mission::{CommonMission}, mission_scheduler::MissionScheduler
 };
 
-
-fn run_ros_topics(scheduler: &MissionScheduler) -> Node {
+fn add_ros_topics(scheduler: &MissionScheduler) -> Node {
     let ctx = r2r::Context::create().expect("Failed to create r2r context!");
     let mut node = r2r::Node::create(ctx, "mission_planner", "namespace")
         .expect("Failed to get Node!");
 
+    
+    node
+}
+
+fn add_example_ros_topics(scheduler: &MissionScheduler, node: &mut Node) {
     let mut example_sub = node
         .subscribe::<sensor_msgs::msg::Image>("/camera/image", QosProfile::default())
         .expect("Failed to create example subscriber!");
@@ -77,46 +81,56 @@ fn run_ros_topics(scheduler: &MissionScheduler) -> Node {
 
     scheduler.add_async_thread(example_publisher_func);
     scheduler.add_async_thread(example_subscriber_func);
-
-    node
 }
     
 fn main() {
-    let pytest = c_str!(include_str!("pymission_example.py"));
-    let pymission_example = pymission::get_mission_from(pytest, c_str!("pymission_example.py"));
 
-    let cmission_example;
-    unsafe {
-        let cmission_ptr = cmission_example_create();
-        cmission_example = *Box::from_raw(cmission_ptr as *mut CommonMission);
-    }
+}
 
-    let foo = mission_example::new();
-    let bar = concurrent_mission_example::new();
-
-    let mission_list: [MissionBox; 3] = [
-        Box::new(pymission_example),
-        Box::new(cmission_example),
-        Box::new(foo),
-    ];
-    let mission_list = VecDeque::from(mission_list);
-    let conc_mission_list: [MissionBox; 1] = [
-        Box::new(bar)
-    ];
-    let conc_mission_list = VecDeque::from(conc_mission_list);
-
-    let scheduler = MissionScheduler::start();
-    scheduler.append(mission_list);
-    scheduler.conc_append(conc_mission_list);
-    let _data = scheduler.get_data();
-
-    let start = Instant::now();
-    scheduler.run();
-    let mut node = run_ros_topics(&scheduler);
-    while start.elapsed() < Duration::from_secs(15) {
-        node.spin_once(Duration::from_millis(100));
-    }
-    scheduler.stop();
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn main_test() -> Result<(), String> {
+        let pytest = c_str!(include_str!("pymission_example.py"));
+        let pymission_example = pymission::get_mission_from(pytest, c_str!("pymission_example.py"));
+
+        let cmission_example;
+        unsafe {
+            let cmission_ptr = cmission_example_create();
+            cmission_example = *Box::from_raw(cmission_ptr as *mut CommonMission);
+        }
+
+        let foo = mission_example::new();
+        let bar = concurrent_mission_example::new();
+
+        let mission_list: [MissionBox; 3] = [
+            Box::new(pymission_example),
+            Box::new(cmission_example),
+            Box::new(foo),
+        ];
+        let mission_list = VecDeque::from(mission_list);
+        let conc_mission_list: [MissionBox; 1] = [
+            Box::new(bar)
+        ];
+        let conc_mission_list = VecDeque::from(conc_mission_list);
+
+        let scheduler = MissionScheduler::start();
+        scheduler.append(mission_list);
+        scheduler.conc_append(conc_mission_list);
+        let _data = scheduler.get_data();
+
+        let start = Instant::now();
+        scheduler.run();
+        let mut node = add_ros_topics(&scheduler);
+        add_example_ros_topics(&scheduler, &mut node);
+        while start.elapsed() < Duration::from_secs(15) {
+            node.spin_once(Duration::from_millis(100));
+        }
+        scheduler.stop();
+        Ok(())
 
 
+    }
 }

From aedb0a79808f2f383d0f02ec0db35d191f7990f9 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Mon, 20 Oct 2025 16:45:36 -0400
Subject: [PATCH 17/19] WIP: Add interfaces

---
 src/interfaces/CMakeLists.txt      | 30 ++++++++++++++++++++++++++++++
 src/interfaces/msg/Map.msg         |  2 ++
 src/interfaces/msg/MapObject.msg   |  2 ++
 src/interfaces/msg/MovingState.msg |  3 +++
 src/interfaces/package.xml         | 28 ++++++++++++++++++++++++++++
 5 files changed, 65 insertions(+)
 create mode 100644 src/interfaces/CMakeLists.txt
 create mode 100644 src/interfaces/msg/Map.msg
 create mode 100644 src/interfaces/msg/MapObject.msg
 create mode 100644 src/interfaces/msg/MovingState.msg
 create mode 100644 src/interfaces/package.xml

diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt
new file mode 100644
index 0000000..30dd73d
--- /dev/null
+++ b/src/interfaces/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.10)
+project(interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(vision_msgs REQUIRED)
+
+# Generate messages and actions
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/MovingState.msg"
+  "msg/Map.msg"
+  "msg/MapObject.msg"
+  DEPENDENCIES vision_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/interfaces/msg/Map.msg b/src/interfaces/msg/Map.msg
new file mode 100644
index 0000000..f5da194
--- /dev/null
+++ b/src/interfaces/msg/Map.msg
@@ -0,0 +1,2 @@
+vision_msgs/BoundingBox3D map_bounds
+MapObject[] objects
diff --git a/src/interfaces/msg/MapObject.msg b/src/interfaces/msg/MapObject.msg
new file mode 100644
index 0000000..3ae79e4
--- /dev/null
+++ b/src/interfaces/msg/MapObject.msg
@@ -0,0 +1,2 @@
+int32 cls
+vision_msgs/BoundingBox3D bbox
diff --git a/src/interfaces/msg/MovingState.msg b/src/interfaces/msg/MovingState.msg
new file mode 100644
index 0000000..62d5fa1
--- /dev/null
+++ b/src/interfaces/msg/MovingState.msg
@@ -0,0 +1,3 @@
+bool depth
+bool rotation
+bool linear
diff --git a/src/interfaces/package.xml b/src/interfaces/package.xml
new file mode 100644
index 0000000..e24286b
--- /dev/null
+++ b/src/interfaces/package.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>interfaces</name>
+  <version>0.0.0</version>
+  <description>Custom ROS 2 messages for my project</description>
+  <maintainer email="101651981+joseburgosguntin@users.noreply.github.com">joseburgosguntin</maintainer>
+  <license>TODO: License declaration</license>
+
+  <!-- Build tool -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <!-- Runtime dependencies -->
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>vision_msgs</exec_depend>
+
+  <!-- Testing -->
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <!-- Mark as interface package -->
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

From 3dd8a5ebf67eeb1ba0ec2b6c01f2a44d64e05981 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Mon, 20 Oct 2025 17:04:29 -0400
Subject: [PATCH 18/19] mission-planner: Move add_example_ros_topics to tests

---
 src/mission-planner/src/main.rs | 110 ++++++++++++++++----------------
 1 file changed, 56 insertions(+), 54 deletions(-)

diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index 51f6ce7..0ea13bf 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -29,60 +29,9 @@ fn add_ros_topics(scheduler: &MissionScheduler) -> Node {
     let mut node = r2r::Node::create(ctx, "mission_planner", "namespace")
         .expect("Failed to get Node!");
 
-    
     node
 }
 
-fn add_example_ros_topics(scheduler: &MissionScheduler, node: &mut Node) {
-    let mut example_sub = node
-        .subscribe::<sensor_msgs::msg::Image>("/camera/image", QosProfile::default())
-        .expect("Failed to create example subscriber!");
-    let example_pub= node
-        .create_publisher::<std_msgs::msg::String>("/example", QosProfile::default())
-        .expect("Failed to create example publisher!");
-
-    let example_subscriber_func = 
-    |thread_data : Arc<MissionThreadData>| {
-        let scheduler_data = thread_data.clone();    
-        let pin: BoxFuture<'static, ()> = Box::pin(async move {
-            while ! scheduler_data.stop.load(Ordering::Relaxed) {
-                match example_sub.next().await {
-                    Some(msg) => {
-                        let mission = ros_mission::new(msg);
-                        scheduler_data.push_back(mission);
-                    }
-                    None => break,
-                }
-            }
-        });
-        pin
-    };
-
-    let example_publisher_func = 
-    |thread_data : Arc<MissionThreadData>| {
-        let scheduler_data = thread_data.clone();
-        let pin: BoxFuture<'static, ()> = Box::pin(async move {
-            let mut counter = 0;
-            let mut stop = false;
-            while ! stop {
-                let msg = std_msgs::msg::String {
-                    data: format!("{}", counter),
-                };
-                example_pub.publish(&msg).expect("Failed to publish example!");
-                counter += 1;
-                stop = scheduler_data.stop.load(Ordering::Relaxed);
-                //Should we use a ros timer instead?
-                sleep(Duration::from_secs(1));
-                //This should probably go on another thread
-            }
-        });
-        pin
-    };
-
-    scheduler.add_async_thread(example_publisher_func);
-    scheduler.add_async_thread(example_subscriber_func);
-}
-    
 fn main() {
 
 }
@@ -91,10 +40,64 @@ fn main() {
 mod tests {
     use super::*;
 
+    fn add_example_ros_topics(scheduler: &MissionScheduler) -> Node {
+        let ctx = r2r::Context::create().expect("Failed to create r2r context!");
+        let mut node = r2r::Node::create(ctx, "mission_planner", "namespace")
+            .expect("Failed to get Node!");
+        let mut example_sub = node
+            .subscribe::<sensor_msgs::msg::Image>("/camera/image", QosProfile::default())
+            .expect("Failed to create example subscriber!");
+        let example_pub= node
+            .create_publisher::<std_msgs::msg::String>("/example", QosProfile::default())
+            .expect("Failed to create example publisher!");
+
+        let example_subscriber_func =
+        |thread_data : Arc<MissionThreadData>| {
+            let scheduler_data = thread_data.clone();  
+            let pin: BoxFuture<'static, ()> = Box::pin(async move {
+                while ! scheduler_data.stop.load(Ordering::Relaxed) {
+                    match example_sub.next().await {
+                        Some(msg) => {
+                            let mission = ros_mission::new(msg);
+                            scheduler_data.push_back(mission);
+                        }
+                        None => break,
+                    }
+                }
+            });
+            pin
+        };
+
+        let example_publisher_func =
+        |thread_data : Arc<MissionThreadData>| {
+            let scheduler_data = thread_data.clone();
+            let pin: BoxFuture<'static, ()> = Box::pin(async move {
+                let mut counter = 0;
+                let mut stop = false;
+                while ! stop {
+                    let msg = std_msgs::msg::String {
+                        data: format!("{}", counter),
+                    };
+                    example_pub.publish(&msg).expect("Failed to publish example!");
+                    counter += 1;
+                    stop = scheduler_data.stop.load(Ordering::Relaxed);
+                    //Should we use a ros timer instead?
+                    sleep(Duration::from_secs(1));
+                    //This should probably go on another thread
+                }
+            });
+            pin
+        };
+
+        scheduler.add_async_thread(example_publisher_func);
+        scheduler.add_async_thread(example_subscriber_func);
+        node
+    }
+
     #[test]
     fn main_test() -> Result<(), String> {
         let pytest = c_str!(include_str!("pymission_example.py"));
-        let pymission_example = pymission::get_mission_from(pytest, c_str!("pymission_example.py"));
+        let pymission_example = pymissfion::get_mission_from(pytest, c_str!("pymission_example.py"));
 
         let cmission_example;
         unsafe {
@@ -123,8 +126,7 @@ mod tests {
 
         let start = Instant::now();
         scheduler.run();
-        let mut node = add_ros_topics(&scheduler);
-        add_example_ros_topics(&scheduler, &mut node);
+        let mut node = add_example_ros_topics(&scheduler);
         while start.elapsed() < Duration::from_secs(15) {
             node.spin_once(Duration::from_millis(100));
         }

From eb9a9e77a1e89445b2ad8c79ba9193c97bb4bbb7 Mon Sep 17 00:00:00 2001
From: Ultra <ultra.public@proton.me>
Date: Wed, 22 Oct 2025 22:25:44 -0400
Subject: [PATCH 19/19] WIP: Some stuff

---
 src/mission-planner/Cargo.toml                |   2 +-
 src/mission-planner/build.rs                  |   9 +-
 .../src/{ => examples}/cmission_example.c     |   2 +-
 .../src/{ => examples}/cmission_example.h     |   0
 .../concurrent_mission_example.rs             |   0
 .../src/{ => examples}/mission_example.rs     |   0
 src/mission-planner/src/examples/mod.rs       |   3 +
 .../src/{ => examples}/pymission_example.py   |   0
 .../src/{ => examples}/ros_mission.rs         |   0
 src/mission-planner/src/main.rs               | 110 ++++++++++++++++--
 src/mission-planner/src/mission.rs            |  12 +-
 src/mission-planner/src/mission_scheduler.rs  |   2 +-
 12 files changed, 124 insertions(+), 16 deletions(-)
 rename src/mission-planner/src/{ => examples}/cmission_example.c (95%)
 rename src/mission-planner/src/{ => examples}/cmission_example.h (100%)
 rename src/mission-planner/src/{ => examples}/concurrent_mission_example.rs (100%)
 rename src/mission-planner/src/{ => examples}/mission_example.rs (100%)
 create mode 100644 src/mission-planner/src/examples/mod.rs
 rename src/mission-planner/src/{ => examples}/pymission_example.py (100%)
 rename src/mission-planner/src/{ => examples}/ros_mission.rs (100%)

diff --git a/src/mission-planner/Cargo.toml b/src/mission-planner/Cargo.toml
index ae27759..869beb1 100644
--- a/src/mission-planner/Cargo.toml
+++ b/src/mission-planner/Cargo.toml
@@ -4,7 +4,7 @@ version = "0.1.0"
 edition = "2024"
 
 [dependencies]
-cv-bridge = { version = "0.3.4", path = "../../third_party/cv-bridge-rs", optional = true }
+cv-bridge = { version = "0.3.4", path = "../../third_party/cv-bridge-rs"}
 futures = { version = "0.3.31", features = ["thread-pool"] }
 opencv = { version = "0.95.1", features = ["clang-runtime"] }
 r2r = "0.9.5"
diff --git a/src/mission-planner/build.rs b/src/mission-planner/build.rs
index 7f5ef66..fe66201 100644
--- a/src/mission-planner/build.rs
+++ b/src/mission-planner/build.rs
@@ -2,8 +2,8 @@ use std::env;
 use std::path::PathBuf;
 
 fn main() {
-    println!("cargo::rerun-if-changed=src/cmission_example.c");
-    println!("cargo::rerun-if-changed=src/cmission_example.h");
+    println!("cargo::rerun-if-changed=src/examples/cmission_example.c");
+    println!("cargo::rerun-if-changed=src/examples/cmission_example.h");
     let crate_dir = env::var("CARGO_MANIFEST_DIR").unwrap();
     let mut config: cbindgen::Config = Default::default();
     config.language = cbindgen::Language::C;
@@ -18,7 +18,7 @@ fn main() {
         .write_to_file("bindings.h");
 
     let bindings = bindgen::Builder::default()
-        .header("src/cmission_example.h")
+        .header("src/examples/cmission_example.h")
         .parse_callbacks(Box::new(bindgen::CargoCallbacks::new()))
         .generate()
         .expect("Unable to generate bindings");
@@ -28,8 +28,9 @@ fn main() {
         .write_to_file(out_path.join("bindings.rs"))
         .expect("Couldn't write bindings!");
 
+    //TODO: How do we conditionally build this?
     cc::Build::new()
-        .file("src/cmission_example.c")
+        .file("src/examples/cmission_example.c")
         .compile("cmission_example.o");
 
 }
diff --git a/src/mission-planner/src/cmission_example.c b/src/mission-planner/src/examples/cmission_example.c
similarity index 95%
rename from src/mission-planner/src/cmission_example.c
rename to src/mission-planner/src/examples/cmission_example.c
index 73b2a86..371103c 100644
--- a/src/mission-planner/src/cmission_example.c
+++ b/src/mission-planner/src/examples/cmission_example.c
@@ -1,4 +1,4 @@
-#include "../bindings.h"
+#include "../../bindings.h"
 #include "cmission_example.h"
 
 #include "stdio.h"
diff --git a/src/mission-planner/src/cmission_example.h b/src/mission-planner/src/examples/cmission_example.h
similarity index 100%
rename from src/mission-planner/src/cmission_example.h
rename to src/mission-planner/src/examples/cmission_example.h
diff --git a/src/mission-planner/src/concurrent_mission_example.rs b/src/mission-planner/src/examples/concurrent_mission_example.rs
similarity index 100%
rename from src/mission-planner/src/concurrent_mission_example.rs
rename to src/mission-planner/src/examples/concurrent_mission_example.rs
diff --git a/src/mission-planner/src/mission_example.rs b/src/mission-planner/src/examples/mission_example.rs
similarity index 100%
rename from src/mission-planner/src/mission_example.rs
rename to src/mission-planner/src/examples/mission_example.rs
diff --git a/src/mission-planner/src/examples/mod.rs b/src/mission-planner/src/examples/mod.rs
new file mode 100644
index 0000000..cf5899d
--- /dev/null
+++ b/src/mission-planner/src/examples/mod.rs
@@ -0,0 +1,3 @@
+pub mod mission_example;
+pub mod concurrent_mission_example;
+pub mod ros_mission;
\ No newline at end of file
diff --git a/src/mission-planner/src/pymission_example.py b/src/mission-planner/src/examples/pymission_example.py
similarity index 100%
rename from src/mission-planner/src/pymission_example.py
rename to src/mission-planner/src/examples/pymission_example.py
diff --git a/src/mission-planner/src/ros_mission.rs b/src/mission-planner/src/examples/ros_mission.rs
similarity index 100%
rename from src/mission-planner/src/ros_mission.rs
rename to src/mission-planner/src/examples/ros_mission.rs
diff --git a/src/mission-planner/src/main.rs b/src/mission-planner/src/main.rs
index 0ea13bf..882417e 100644
--- a/src/mission-planner/src/main.rs
+++ b/src/mission-planner/src/main.rs
@@ -2,9 +2,7 @@ mod mission;
 mod mission_scheduler;
 mod cmission;
 mod pymission;
-mod mission_example;
-mod concurrent_mission_example;
-mod ros_mission;
+mod examples;
 
 include!(concat!(env!("OUT_DIR"), "/bindings.rs"));
 
@@ -16,29 +14,125 @@ use std::time::{Duration, Instant};
 
 use futures::prelude::*;
 use futures::future::BoxFuture;
-use pyo3::{ffi::c_str};
-use r2r::{Node, QosProfile, sensor_msgs, std_msgs};
+use r2r::{Node, QosProfile, interfaces, nav_msgs, std_msgs};
+use interfaces::msg::*;
 
+use crate::mission::MissionData;
 use crate::mission_scheduler::{MissionBox, MissionThreadData};
 use crate::{
     mission::{CommonMission}, mission_scheduler::MissionScheduler
 };
 
+type OdometrySub = nav_msgs::msg::Odometry;
+type Float64MultiArray = std_msgs::msg::Float64MultiArray;
+
+
+fn handle_map(data: &MissionData, map: Map) {
+    let mut cached_map = data.cached_map.try_lock().expect("Failed to lock cached map");
+    *cached_map = map;
+    let map = &*cached_map;
+
+    if !data.scouting.load(Ordering::Relaxed) {
+        return;
+    }
+    let map_objects_count = data.map_objects_count.load(Ordering::Relaxed);
+    let new_objects_count = map.objects.len() - map_objects_count;
+    for i in map_objects_count..new_objects_count {
+        let new_object = &map.objects[i];
+        let pos = &new_object.bbox.center.position;
+        r2r::log_info!("mission_planner", "MapObject {} {{ {} {} {} }}", &new_object.cls, &pos.x, &pos.y, &pos.z);
+        //Dunno how to implement object detection yet
+
+    }
+    data.map_objects_count.store(map_objects_count+1, Ordering::Relaxed);
+
+}
+
+fn handle_odometry(data: &MissionData, odometry: OdometrySub) {
+    let p = odometry.pose.pose.position;
+    let o = odometry.pose.pose.orientation;
+
+    let mut cur_pos = data.pose.try_lock().expect("Failed to lock pose");
+    cur_pos.position = p;
+    cur_pos.orientation = o;
+
+    r2r::log_info!("mission_planner", "Odometry {:#?} {:#?}", cur_pos.position, cur_pos.orientation);
+
+    //Skipped yaw stuff
+}
+
 fn add_ros_topics(scheduler: &MissionScheduler) -> Node {
     let ctx = r2r::Context::create().expect("Failed to create r2r context!");
     let mut node = r2r::Node::create(ctx, "mission_planner", "namespace")
         .expect("Failed to get Node!");
+    //What QoS should we use?
+    let mut map_sub = node
+        .subscribe::<Map>("/hydrus/map", QosProfile::default())
+        .expect("Failed to subscribe to map");
+    let mut odometry_sub = node
+        .subscribe::<OdometrySub>("/hydrus/odometry", QosProfile::default())
+        .expect("Failed to subscribe to odometry");
+    let thrusters_pub = node
+        .create_publisher::<Float64MultiArray>("/hydrus/trusters", QosProfile::default())
+        .expect("Failed to setup thruster publisher");
+
+    let map_sub_func =
+    |thread_data : Arc<MissionThreadData>| {
+        let thread_data = thread_data.clone();
+        let pin: BoxFuture<'static, ()> = Box::pin(async move {
+            let data = &thread_data.mission_data;
+            while ! thread_data.stop.load(Ordering::Relaxed) {
+                match map_sub.next().await {
+                    Some(map) => {
+                        handle_map(data, map);
+                    }
+                    None => break,
+                }
+            }
+        });
+        pin
+    };
+
+    let odometry_sub_func =
+    |thread_data : Arc<MissionThreadData>| {
+        let thread_data = thread_data.clone();
+        let pin: BoxFuture<'static, ()> = Box::pin(async move {
+            let data = &thread_data.mission_data;
+            while ! thread_data.stop.load(Ordering::Relaxed) {
+                match odometry_sub.next().await {
+                    Some(odometry) => {
+                        handle_odometry(data, odometry);
+                    }
+                    None => break,
+                }
+            }
+        });
+        pin
+    };
+
+    scheduler.add_async_thread(map_sub_func);
+    scheduler.add_async_thread(odometry_sub_func);
 
     node
 }
 
 fn main() {
+    let scheduler = MissionScheduler::start();
+    let mut node = add_ros_topics(&scheduler);
 
+    scheduler.run();
+    let start = Instant::now();
+    while start.elapsed() < Duration::from_secs(15) {
+        node.spin_once(Duration::from_millis(100));
+    }
 }
 
 #[cfg(test)]
 mod tests {
     use super::*;
+    use crate::examples::*;
+    use r2r::sensor_msgs;
+    use pyo3::{ffi::c_str};
 
     fn add_example_ros_topics(scheduler: &MissionScheduler) -> Node {
         let ctx = r2r::Context::create().expect("Failed to create r2r context!");
@@ -53,7 +147,7 @@ mod tests {
 
         let example_subscriber_func =
         |thread_data : Arc<MissionThreadData>| {
-            let scheduler_data = thread_data.clone();  
+            let scheduler_data = thread_data.clone();
             let pin: BoxFuture<'static, ()> = Box::pin(async move {
                 while ! scheduler_data.stop.load(Ordering::Relaxed) {
                     match example_sub.next().await {
@@ -96,8 +190,8 @@ mod tests {
 
     #[test]
     fn main_test() -> Result<(), String> {
-        let pytest = c_str!(include_str!("pymission_example.py"));
-        let pymission_example = pymissfion::get_mission_from(pytest, c_str!("pymission_example.py"));
+        let pytest = c_str!(include_str!("examples/pymission_example.py"));
+        let pymission_example = pymission::get_mission_from(pytest, c_str!("pymission_example.py"));
 
         let cmission_example;
         unsafe {
diff --git a/src/mission-planner/src/mission.rs b/src/mission-planner/src/mission.rs
index d8d5429..5fbe95a 100644
--- a/src/mission-planner/src/mission.rs
+++ b/src/mission-planner/src/mission.rs
@@ -1,4 +1,6 @@
-use std::sync::atomic::AtomicBool;
+use std::sync::{Mutex, atomic::{AtomicBool, AtomicI32, AtomicUsize}};
+use r2r::interfaces::msg::*;
+use r2r::geometry_msgs::msg::Pose;
 pub type MissionResult = Result<(), bool>;
 
 #[derive(Debug)]
@@ -6,6 +8,10 @@ pub type MissionResult = Result<(), bool>;
 pub struct MissionData {
     pub example_flag : AtomicBool,
     pub example_flag_request : AtomicBool,
+    pub cached_map : Mutex<Map>,
+    pub scouting : AtomicBool,
+    pub map_objects_count : AtomicUsize, //It is possible this being atomic could cause problems
+    pub pose : Mutex<Pose>,
 }
 
 impl MissionData {
@@ -13,6 +19,10 @@ impl MissionData {
         MissionData {
             example_flag: AtomicBool::new(false),
             example_flag_request: AtomicBool::new(false),
+            cached_map: Mutex::new(Map::default()),
+            scouting: AtomicBool::new(true),
+            map_objects_count: AtomicUsize::new(0),
+            pose : Mutex::new(Pose::default())
         }
     }
 }
diff --git a/src/mission-planner/src/mission_scheduler.rs b/src/mission-planner/src/mission_scheduler.rs
index afad9dc..63a0e77 100644
--- a/src/mission-planner/src/mission_scheduler.rs
+++ b/src/mission-planner/src/mission_scheduler.rs
@@ -11,7 +11,7 @@ pub type MissionVec = VecDeque<MissionBox>;
 pub struct MissionThreadData {
     mission_list: Mutex<MissionVec>,
     conc_mission_list: Mutex<MissionVec>,
-    mission_data: MissionData,
+    pub mission_data: MissionData,
     run: AtomicBool,
     pub stop: AtomicBool,
     waiting: AtomicBool,