config file finished

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"

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"

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>

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)