From c93cbbf3be589f7bf808020a2e36501726326b42 Mon Sep 17 00:00:00 2001
From: Mehul Goel <mehulgoel873@gmail.com>
Date: Wed, 8 Jan 2025 21:51:07 -0800
Subject: [PATCH] config file finished

---
 rb_ws/src/buggy/config/sim_double.yaml        | 34 +++++++++++++++++++
 rb_ws/src/buggy/config/sim_single_sc.yaml     | 20 +++++------
 rb_ws/src/buggy/launch/sim_2d_double.xml      | 19 ++++-------
 .../buggy/scripts/buggy_state_converter.py    |  4 +++
 4 files changed, 55 insertions(+), 22 deletions(-)
 create mode 100644 rb_ws/src/buggy/config/sim_double.yaml

diff --git a/rb_ws/src/buggy/config/sim_double.yaml b/rb_ws/src/buggy/config/sim_double.yaml
new file mode 100644
index 0000000..af2e085
--- /dev/null
+++ b/rb_ws/src/buggy/config/sim_double.yaml
@@ -0,0 +1,34 @@
+/**: # Global Params
+  ros__parameters:
+      traj_name: "buggycourse_safe.json"
+
+NAND:
+  NAND_sim_single:
+    ros__parameters:
+      velocity: 10
+      pose: "Hill1_NAND"
+
+  NAND_controller:
+    ros__parameters:
+      dist: 0.0
+      # traj_name: "buggycourse_safe.json"
+      controller: "stanley"
+
+SC:
+  SC_sim_single:
+    ros__parameters:
+      velocity: 12
+      pose: "Hill1_SC"
+
+SC:
+  SC_controller:
+    ros__parameters:
+      dist: 0.0
+      # traj_name: "buggycourse_safe.json"
+      controller: "stanley"
+
+SC:
+  SC_path_planner:
+    ros__parameters:
+      traj_name: "buggycourse_safe.json"
+      curb_name: "buggycourse_curb.json"
diff --git a/rb_ws/src/buggy/config/sim_single_sc.yaml b/rb_ws/src/buggy/config/sim_single_sc.yaml
index 4b8089a..5d9b8cf 100644
--- a/rb_ws/src/buggy/config/sim_single_sc.yaml
+++ b/rb_ws/src/buggy/config/sim_single_sc.yaml
@@ -1,10 +1,10 @@
-/SC/SC_sim_single:
-   ros__parameters:
-      velocity: 12
-      pose: "Hill1_SC"
-
-/SC/SC_controller:
-   ros__parameters:
-      dist: 10.0
-      traj_name: "buggycourse_safe.json"
-      controller: "stanley"
+SC:
+   SC_sim_single:
+      ros__parameters:
+         velocity: 12
+         pose: "Hill1_SC"
+   SC_controller:
+      ros__parameters:
+         dist: 0.0
+         traj_name: "buggycourse_safe.json"
+         controller: "stanley"
diff --git a/rb_ws/src/buggy/launch/sim_2d_double.xml b/rb_ws/src/buggy/launch/sim_2d_double.xml
index b9d5ba6..545f5aa 100644
--- a/rb_ws/src/buggy/launch/sim_2d_double.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_double.xml
@@ -1,31 +1,26 @@
 <launch>
 
+    <arg name="config_file" default="src/buggy/config/sim_double.yaml"/>
+
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" output = "screen" namespace="SC"/>
 
     <node pkg="buggy" exec="buggy_state_converter.py" name="NAND_state_converter" output = "screen" namespace="NAND"/>
     <node pkg="buggy" exec="controller_node.py" name="NAND_controller" output = "screen" namespace="NAND">
-        <param name="dist" value="0.0"/>
-        <param name="traj_name" value="buggycourse_safe.json"/>
-        <param name="controller" value="stanley"/>
+        <param from="$(var config_file)"/>
     </node>
     <node pkg="buggy" exec="engine.py" name="NAND_sim_single" output = "screen" namespace="NAND">
-        <param name="velocity" value="8"/>
-        <param name="pose" value="Hill1_NAND"/>
+        <param from="$(var config_file)"/>
     </node>
     
     <node pkg="buggy" exec="buggy_state_converter.py" name="SC_state_converter" output = "screen" namespace="SC"/>
     <node pkg="buggy" exec="controller_node.py" name="SC_controller" output = "screen" namespace="SC">
-        <param name="dist" value="0.0"/>
-        <param name="traj_name" value="buggycourse_safe.json"/>
-        <param name="controller" value="stanley"/>
+        <param from="$(var config_file)"/>
     </node>
     <node pkg="buggy" exec="engine.py" name="SC_sim_single" output = "screen" namespace="SC">
-        <param name="velocity" value="12"/>
-        <param name="pose" value="Hill1_SC"/>
+        <param from="$(var config_file)"/>
     </node>
     <node pkg="buggy" exec="path_planner.py" name="SC_path_planner" namespace="SC" output = "screen">
-        <param name="traj_name" value="buggycourse_safe.json"/>
-        <remap from="other/state" to="/NAND/self/state"/>
+        <param from="$(var config_file)"/>
     </node>
 
 </launch>
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/buggy_state_converter.py b/rb_ws/src/buggy/scripts/buggy_state_converter.py
index 48b1086..eab0cba 100755
--- a/rb_ws/src/buggy/scripts/buggy_state_converter.py
+++ b/rb_ws/src/buggy/scripts/buggy_state_converter.py
@@ -10,6 +10,7 @@
 class BuggyStateConverter(Node):
     def __init__(self):
         super().__init__("buggy_state_converter")
+        self.get_logger().info('INITIALIZED.')
 
         namespace = self.get_namespace()
 
@@ -35,6 +36,8 @@ def self_raw_state_callback(self, msg):
         """ Callback for processing self/raw_state messages and publishing to self/state """
         namespace = self.get_namespace()
 
+        self.get_logger().error("CALLED_SELF_CALLBACK")
+
         if namespace == "/SC":
             converted_msg = self.convert_SC_state(msg)
         elif namespace == "/NAND":
@@ -48,6 +51,7 @@ def self_raw_state_callback(self, msg):
     def other_raw_state_callback(self, msg):
         """ Callback for processing other/raw_state messages and publishing to other/state """
         # Convert the SC message and publish to other/state
+        self.get_logger().error("CALLED_OTHER_CALLBACK")
         converted_msg = self.convert_NAND_other_state(msg)
         self.other_state_publisher.publish(converted_msg)