From 2734055ef6bd0974ef75369cdbf555e21cecfc57 Mon Sep 17 00:00:00 2001
From: kpbaks <kristoffer.pbs@gmail.com>
Date: Wed, 27 Mar 2024 11:40:20 +0100
Subject: [PATCH] fix: forgot to add file

---
 gbpplanner-rs/src/planner/debug.rs | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)
 create mode 100644 gbpplanner-rs/src/planner/debug.rs

diff --git a/gbpplanner-rs/src/planner/debug.rs b/gbpplanner-rs/src/planner/debug.rs
new file mode 100644
index 00000000..875cc07f
--- /dev/null
+++ b/gbpplanner-rs/src/planner/debug.rs
@@ -0,0 +1,23 @@
+use bevy::prelude::*;
+
+use super::{FactorGraph, RobotState};
+
+pub struct PlannerDebugPlugin;
+
+impl Plugin for PlannerDebugPlugin {
+    fn build(&self, app: &mut App) {
+        app.add_systems(Update, debug);
+    }
+}
+
+fn debug(q: Query<(Entity, &RobotState, &FactorGraph)>) {
+    for (entity, state, graph) in q.iter() {
+        // println!("{:?}", entity);
+        // println!("{:?}", state);
+        // println!("{:?}", graph);
+        graph.variables().for_each(|(index, variable)| {
+            // println!("variable {:?}: mu {}", index, variable.belief.mu);
+        })
+    }
+    // std::process
+}