From 9b55a22ba67e57123ab6d8e68831ab2c33aaf90e Mon Sep 17 00:00:00 2001
From: Mehul Goel <mehulgoel873@gmail.com>
Date: Mon, 15 Jul 2024 15:35:07 -0700
Subject: [PATCH] fixed sim_single

---
 rb_ws/src/buggy/config/sim_single.yaml        | 15 ++++
 rb_ws/src/buggy/launch/sim_single.launch      | 17 ++--
 rb_ws/src/buggy/scripts/2d_sim/engine.py      |  6 +-
 rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py |  4 +-
 .../buggy/scripts/2d_sim/velocity_updater.py  |  4 +-
 rb_ws/src/buggy/scripts/auton/autonsystem.py  | 77 +++++--------------
 .../scripts/auton/rolling_sanity_check.py     |  2 +-
 7 files changed, 49 insertions(+), 76 deletions(-)
 create mode 100644 rb_ws/src/buggy/config/sim_single.yaml
 mode change 100644 => 100755 rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py

diff --git a/rb_ws/src/buggy/config/sim_single.yaml b/rb_ws/src/buggy/config/sim_single.yaml
new file mode 100644
index 00000000..57e1a2ea
--- /dev/null
+++ b/rb_ws/src/buggy/config/sim_single.yaml
@@ -0,0 +1,15 @@
+start_pos: "Hill1_SC"
+velocity: 15.0
+buggy_name: "SC"
+
+manual_vel: false
+auto_vel: false
+
+
+# Auton Arguments
+controller: "stanley"
+dist: 0.0
+traj: "buggycourse_safe.json"
+self_name: "SC"
+other_name: NONE
+
diff --git a/rb_ws/src/buggy/launch/sim_single.launch b/rb_ws/src/buggy/launch/sim_single.launch
index 5cef4eef..1c7d2c27 100755
--- a/rb_ws/src/buggy/launch/sim_single.launch
+++ b/rb_ws/src/buggy/launch/sim_single.launch
@@ -1,8 +1,5 @@
 <launch>
-    <arg name="start_pos"        default="Hill1_SC" />
-    <arg name="autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe.json --self_name SC" />
-    <arg name="velocity"         default="15.0" />
-    <arg name="buggy_name"       default="SC" />
+    <rosparam file="src/buggy/config/sim_single.yaml" />
 
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
 
@@ -12,21 +9,19 @@
     <group if="$(arg auto_vel)">
         <!-- Run the auto velocity updater -->
         <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
-            output="screen" args="$(arg velocity) SC"/>
+            output="screen"/>
     </group>
 
     <group if="$(arg manual_vel)">
         <!-- Run the manual velocity updater -->
         <node name="sc_velocity_ui" pkg="buggy" type="velocity_ui.py"
-            output="screen" args="$(arg velocity) SC"/>
+            output="screen"/>
     </group>
 
-    <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg start_pos) $(arg velocity) $(arg buggy_name)"/>
+    <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"/>
 
-    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen"
-        args="$(arg autonsystem_args)"/>
+    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen"/>
 
-    <node name="rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args="$(arg buggy_name)"/>
+    <node name="rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args=""/>
 
 </launch>
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index cf3f518a..b11f54d9 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -332,9 +332,9 @@ def loop(self):
     print("sim 2d eng args:")
     print(sys.argv)
 
-    start_pos = sys.argv[1]
-    velocity = float(sys.argv[2])
-    buggy_name = sys.argv[3]
+    start_pos = rospy.get_param("/start_pos")
+    velocity = float(rospy.get_param("/velocity"))
+    buggy_name = rospy.get_param("/buggy_name")
 
     sim = Simulator(start_pos, velocity, buggy_name)
     for i in range(100):
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
index 09840e3a..f6d8486f 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -35,8 +35,8 @@ def step(self):
 
 if __name__ == "__main__":
     rospy.init_node("velocity_ui")
-    init_vel = float(sys.argv[1])
-    buggy_name = sys.argv[2]
+    init_vel = float(rospy.get_param("/velocity"))
+    buggy_name = rospy.get_param("/SC")
     vel = VelocityUI(init_vel, buggy_name)
     rate = rospy.Rate(100)
     while not rospy.is_shutdown():
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
index a6bebe18..fc26cb14 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py
@@ -64,8 +64,8 @@ def step(self):
 if __name__ == "__main__":
     rospy.init_node("vel_updater")
 
-    init_vel = float(sys.argv[1])
-    buggy_name = sys.argv[2]
+    init_vel = float(rospy.get_param("/velocity"))
+    buggy_name = rospy.get_param("/SC")
     vel = VelocityUpdater(init_vel, buggy_name)
     rate = rospy.Rate(vel.RATE)
 
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index 14a3c6cc..25c06657 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -261,65 +261,28 @@ def planner_tick(self):
         # update local trajectory via path planner
         self.path_planner.compute_traj(self_pose, other_pose)
 
-def init_parser ():
-    """
-        Returns a parser to read launch file arguments to AutonSystem.
-    """
-    parser = argparse.ArgumentParser()
-    parser.add_argument("--controller",
-        type=str,
-        help="set controller type",
-        required=True)
-
-    parser.add_argument(
-        "--dist",
-        type=float,
-        help="start buggy at meters distance along the path",
-        required=True)
-
-    parser.add_argument(
-        "--traj",
-        type=str,
-        help="path to the trajectory file, relative to /rb_ws/src/buggy/paths/",
-        required=True)
-
-    parser.add_argument(
-        "--self_name",
-        type=str,
-        help="name of ego-buggy",
-        required=True)
-
-    parser.add_argument(
-        "--left_curb",
-        type=str,
-        help="Path of curb data, relative to /rb_ws/src/buggy/paths/",
-        default=""
-,        required=False)
-
-    parser.add_argument(
-        "--other_name",
-        type=str,
-        help="name of other buggy, if left unspecified, the autonsystem assumes it is the only buggy on the course",
-        required=False)
-
-    parser.add_argument(
-        "--profile",
-        action='store_true',
-        help="turn on profiling for the path planner")
-    return parser
-
 if __name__ == "__main__":
     rospy.init_node("auton_system")
-    parser = init_parser()
-
-    args, _ = parser.parse_known_args()
-    ctrl = args.controller
-    start_dist = args.dist
-    traj = args.traj
-    self_name = args.self_name
-    other_name = args.other_name
-    profile = args.profile
-    left_curb_file = args.left_curb
+
+    ctrl = rospy.get_param("/controller")
+    start_dist = rospy.get_param("/dist")
+    traj = rospy.get_param("/traj")
+    self_name = rospy.get_param("/self_name")
+
+    if rospy.has_param("/other_name"):
+        other_name = rospy.get_param("/other_name")
+    else:
+        other_name = None
+
+    if rospy.has_param("/profile"):
+        profile = rospy.get_param("/profile")
+    else:
+        profile = None
+    
+    if rospy.has_param("/left_curb"):
+        left_curb_file = rospy.get_param("/left_curb")
+    else:
+        left_curb_file = ""
 
     rospy.loginfo("\n\nStarting Controller: " + str(ctrl) + "\n\n")
     rospy.loginfo("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(traj) + "\n\n")
diff --git a/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py b/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py
old mode 100644
new mode 100755
index eca106aa..2caa7d8e
--- a/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py
+++ b/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py
@@ -200,7 +200,7 @@ def sanity_check(self):
 
 if __name__ == "__main__":
     rospy.init_node("rolling_sanity_check")
-    check = SanityCheck(sys.argv[1])
+    check = SanityCheck(rospy.get_param("/buggy_name"))
     rate = rospy.Rate(100)
 
     while not rospy.is_shutdown():