added rosparams and added namespaces

README.md

@@ -69,13 +69,13 @@ A complete re-write of the old RoboBuggy.
 
 ## 2D Simulation
 - Boot up the docker container
-- Run `roslaunch buggy sim_2d_single.launch` to simulate 1 buggy
-- See `rb_ws/src/buggy/launch/sim_2d_single.launch` to view all available launch options
-- Run `roslaunch buggy sim_2d_2buggies.launch` to simulate 2 buggies
+- Run `roslaunch buggy sim_single.launch` to simulate 1 buggy
+- See `rb_ws/src/buggy/launch/sim_single.launch` to view all available launch options
+- Run `roslaunch buggy sim_2buggies.launch` to simulate 2 buggies
 
 <img width="612" alt="Screenshot 2023-11-13 at 3 18 30 PM" src="https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/45720415/b204aa05-8792-414e-a868-6fbc0d11ab9d">
 
-- See `rb_ws/src/buggy/launch/sim_2d_2buggies.launch` to view all available launch options
+- See `rb_ws/src/buggy/launch/sim_2buggies.launch` to view all available launch options
     - The buggy starting positions can be changed using the `sc_start_pos` and `nand_start_pos` arguments (can pass as a key to a dictionary of preset start positions in engine.py, a single float for starting distance along planned trajectory, or 3 comma-separated floats (utm east, utm north, and heading))
 - To prevent topic name collision, a topic named `t` associated with buggy named `x` have format `x/t`. The names are `SC` and `Nand` in the 2 buggy simulator. In the one buggy simulator, the name can be defined as a launch arg.
 - See [**Foxglove Visualization**](#foxglove-visualization) for visualizing the simulation. Beware that since topic names are user-defined, you will need to adjust the topic names in each panel.

rb_ws/src/buggy/config/sim_2buggies.yaml

@@ -0,0 +1,22 @@
+# SHORT CIRCUIT
+SC_start_pos: "Hill1_SC"
+SC_velocity: 15.0
+
+
+## Auton Arguments
+SC_controller: "stanley"
+SC_dist: 0.0
+SC_traj: "buggycourse_safe.json"
+SC_self_name: "SC"
+SC_other_name: "NAND"
+
+# NAND
+NAND_start_pos: "Hill1_NAND"
+NAND_velocity: 7.0
+
+## Auton Arguments
+NAND_controller: "stanley"
+NAND_dist: 0.0
+NAND_traj: "buggycourse_safe.json"
+NAND_self_name: "NAND"
+NAND_other_name: NONE

rb_ws/src/buggy/config/sim_single.yaml

@@ -1,15 +1,11 @@
-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
+# SHORT CIRCUIT
+SC_start_pos: "Hill1_SC"
+SC_velocity: 7.0
+
+## Auton Arguments
+SC_controller: "stanley"
+SC_dist: 0.0
+SC_traj: "buggycourse_safe.json"
+SC_self_name: "SC"
+SC_other_name: NONE
 

rb_ws/src/buggy/launch/sim_2buggies.launch

@@ -1,52 +1,38 @@
 <launch>
-    <arg name="sc_controller"     default="stanley" />
-    <arg name="sc_start_pos"    default="Hill1_SC" />
-    <arg name="sc_velocity"         default="15.0" />
+    <rosparam file="src/buggy/config/sim_2buggies.yaml" />
 
-    <arg name="nand_controller"     default="stanley" />
-    <arg name="nand_path"           default="buggycourse_safe.json" />
-    <arg name="sc_path"           default="buggycourse_safe.json" />
-    <arg name="nand_start_pos"     default="Hill1_NAND" />
-    <arg name="nand_velocity"       default="7.0" />
+    <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
 
     <arg name="manual_vel"          default="false" />
     <arg name="auto_vel"            default="false" />
 
     <group if="$(arg auto_vel)">
         <!-- Run the auto velocity updater -->
         <node name="nand_vel_updater" pkg="buggy" type="velocity_updater.py"
-            output="screen" args="$(arg nand_velocity) NAND"/>
+            output="screen" args="NAND"/>
         <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
-            output="screen" args="$(arg sc_velocity) SC"/>
+            output="screen" args="SC"/>
     </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 sc_velocity) SC"/>
+            output="screen" args="SC"/>
         <node name="nand_velocity_ui" pkg="buggy" type="velocity_ui.py"
-            output="screen" args="$(arg nand_velocity) NAND"/>
+            output="screen" args="NAND"/>
     </group>
 
-    <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
-
-
+    <!-- NAND -->
     <node name="nand_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg nand_start_pos) $(arg nand_velocity) NAND"/>
-
-    <!-- NAND is not aware of SC -->
-    <arg name="nand_autonsystem_args" default="--controller $(arg nand_controller) --dist 0.0 --traj $(arg nand_path) --self_name NAND" />
+        args="NAND"/>
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
-        args="$(arg nand_autonsystem_args)"/>
-
-    <arg name="sc_autonsystem_args" default="--controller $(arg sc_controller) --dist 0.0 --traj $(arg sc_path) --self_name SC --other_name NAND" />
-    <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
-        args="$(arg sc_autonsystem_args)"/>
+        args="NAND"/>
 
+    <!-- SC -->
     <node name="sc_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg sc_start_pos) $(arg sc_velocity) SC"/>
-
-
-    <node name="sc_rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args="SC"/>
+        args="SC"/>
+    <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
+        args="SC"/>
+    <!-- <node name="sc_rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args="SC"/> -->
 
 </launch>

rb_ws/src/buggy/launch/sim_single.launch

@@ -3,25 +3,29 @@
 
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
 
-    <arg name="manual_vel"          default="false" />
-    <arg name="auto_vel"            default="false" />
+    <arg name="manual_vel"          default="true" />
+    <arg name="auto_vel"            default="true" />
 
-    <group if="$(arg auto_vel)">
-        <!-- Run the auto velocity updater -->
-        <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
-            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"/>
-    </group>
+    <group ns = "SC">
+         <group if="$(arg auto_vel)">
+            <!-- Run the auto velocity updater -->
+            <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
+                output="screen" args="SC"/>
+        </group>
 
-    <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"/>
+        <!-- Might be broken, I am geting a docker issue as of now. -->
+        <group if="$(arg manual_vel)">
+            <!-- Run the manual velocity updater -->
+            <node name="sc_velocity_ui" pkg="buggy" type="velocity_ui.py"
+                output="screen" args = "SC"/> 
+        </group>
 
-    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen"/>
+        <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen" args="SC"/>
 
-    <node name="rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args=""/>
+        <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="SC"/>
+
+        <node name="rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args="SC"/>
+    </group>
 
 </launch>

rb_ws/src/buggy/scripts/2d_sim/controller_2d.py

@@ -6,8 +6,8 @@
 
 class Controller:
     def __init__(self, buggy_name):
-        self.steering_publisher = rospy.Publisher(buggy_name + "/input/steering", Float64, queue_size=10)
-        self.velocity_publisher = rospy.Publisher(buggy_name + "/velocity", Float64, queue_size=10)
+        self.steering_publisher = rospy.Publisher("/input/steering", Float64, queue_size=10)
+        self.velocity_publisher = rospy.Publisher("/velocity", Float64, queue_size=10)
         self.steering_angle = 0
         self.velocity = 0
 

rb_ws/src/buggy/scripts/2d_sim/engine.py

@@ -32,27 +32,27 @@ def __init__(self, start_pos: str, velocity: float, buggy_name: str):
             heading (float): degrees start heading of buggy
         """
         # for X11 matplotlib (direction included)
-        self.plot_publisher = rospy.Publisher(buggy_name + "/sim_2d/utm", Pose, queue_size=1)
+        self.plot_publisher = rospy.Publisher("/sim_2d/utm", Pose, queue_size=1)
 
         # simulate the INS's outputs (noise included)
-        self.pose_publisher = rospy.Publisher(buggy_name + "/nav/odom", Odometry, queue_size=1)
+        self.pose_publisher = rospy.Publisher("/nav/odom", Odometry, queue_size=1)
 
         self.steering_subscriber = rospy.Subscriber(
-            buggy_name + "/buggy/input/steering", Float64, self.update_steering_angle
+            "/buggy/input/steering", Float64, self.update_steering_angle
         )
         # To read from velocity
         self.velocity_subscriber = rospy.Subscriber(
-            buggy_name + "/velocity", Float64, self.update_velocity
+            "/velocity", Float64, self.update_velocity
         )
         # to plot on Foxglove (no noise)
         self.navsatfix_publisher = rospy.Publisher(
-            buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1
+            "/state/pose_navsat", NavSatFix, queue_size=1
         )
 
         if Simulator.NOISE:
             # to plot on Foxglove (with noise)
             self.navsatfix_noisy_publisher = rospy.Publisher(
-                buggy_name + "/state/pose_navsat_noisy", NavSatFix, queue_size=1
+                "/state/pose_navsat_noisy", NavSatFix, queue_size=1
             )
 
         # (UTM east, UTM north, HEADING(degs))
@@ -329,14 +329,13 @@ def loop(self):
 
 if __name__ == "__main__":
     rospy.init_node("sim_2d_engine")
-    print("sim 2d eng args:")
-    print(sys.argv)
+    print("sim 2d eng args: ", sys.argv)
+    self_name = sys.argv[1]
 
-    start_pos = rospy.get_param("/start_pos")
-    velocity = float(rospy.get_param("/velocity"))
-    buggy_name = rospy.get_param("/buggy_name")
+    start_pos = rospy.get_param("/" + self_name + "_start_pos")
+    velocity = float(rospy.get_param("/" + self_name + "_velocity"))
 
-    sim = Simulator(start_pos, velocity, buggy_name)
+    sim = Simulator(start_pos, velocity, self_name)
     for i in range(100):
         sim.publish()
         rospy.sleep(0.01)
@@ -345,7 +344,7 @@ def loop(self):
     # so that auton stack has time to initialize
     # before buggy moves
     print("SIM PUBLISHED", rospy.get_time())
-    if buggy_name == "SC":
+    if self_name == "SC":
         rospy.sleep(15.0)
     print("SIM STARTED STEPPING", rospy.get_time())
     sim.loop()

rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py

@@ -35,9 +35,9 @@ def step(self):
 
 if __name__ == "__main__":
     rospy.init_node("velocity_ui")
-    init_vel = float(rospy.get_param("/velocity"))
-    buggy_name = rospy.get_param("/SC")
-    vel = VelocityUI(init_vel, buggy_name)
+    self_name = sys.argv[1]
+    init_vel = float(rospy.get_param("/" + self_name + "_velocity"))
+    vel = VelocityUI(init_vel, self_name)
     rate = rospy.Rate(100)
     while not rospy.is_shutdown():
         vel.step()

rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py

@@ -63,10 +63,10 @@ def step(self):
 
 if __name__ == "__main__":
     rospy.init_node("vel_updater")
+    self_name = sys.argv[1]
 
-    init_vel = float(rospy.get_param("/velocity"))
-    buggy_name = rospy.get_param("/SC")
-    vel = VelocityUpdater(init_vel, buggy_name)
+    init_vel = float(rospy.get_param("/" + self_name + "_velocity"))
+    vel = VelocityUpdater(init_vel, self_name)
     rate = rospy.Rate(vel.RATE)
 
     while not rospy.is_shutdown():

rb_ws/src/buggy/scripts/auton/autonsystem.py

@@ -2,6 +2,7 @@
 
 import argparse
 from threading import Lock
+import sys
 
 import threading
 import rospy
@@ -68,12 +69,12 @@ def __init__(self,
         self.other_odom_msg = None
         self.use_gps_pos = False
 
-        rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
-        rospy.Subscriber(self_name + "/gnss1/odom", Odometry, self.update_self_odom_backup)
-        rospy.Subscriber(self_name + "/nav/traj", TrajectoryMsg, self.update_traj)
+        rospy.Subscriber("/nav/odom", Odometry, self.update_self_odom)
+        rospy.Subscriber("/gnss1/odom", Odometry, self.update_self_odom_backup)
+        rospy.Subscriber("/nav/traj", TrajectoryMsg, self.update_traj)
 
         # to report if the filter position has separated (so we need to use the antenna position)
-        rospy.Subscriber(self_name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps)
+        rospy.Subscriber("/debug/filter_gps_seperation_status", Bool, self.update_use_gps)
 
         if self.has_other_buggy:
             rospy.Subscriber(other_name + "/nav/odom", Odometry, self.update_other_odom)
@@ -82,16 +83,16 @@ def __init__(self,
             )
 
         self.init_check_publisher = rospy.Publisher(
-            self_name + "/debug/init_safety_check", Bool, queue_size=1
+            "/debug/init_safety_check", Bool, queue_size=1
         )
         self.steer_publisher = rospy.Publisher(
-            self_name + "/buggy/input/steering", Float64, queue_size=1
+            "/buggy/input/steering", Float64, queue_size=1
         )
         self.heading_publisher = rospy.Publisher(
-            self_name + "/auton/debug/heading", Float32, queue_size=1
+            "/auton/debug/heading", Float32, queue_size=1
         )
         self.distance_publisher = rospy.Publisher(
-            self_name + "/auton/debug/distance", Float64, queue_size=1
+            "/auton/debug/distance", Float64, queue_size=1
         )
 
         self.controller_rate = 100
@@ -264,23 +265,23 @@ def planner_tick(self):
 if __name__ == "__main__":
     rospy.init_node("auton_system")
 
-    ctrl = rospy.get_param("/controller")
-    start_dist = rospy.get_param("/dist")
-    traj = rospy.get_param("/traj")
-    self_name = rospy.get_param("/self_name")
+    self_name = sys.argv[1]
+    ctrl = rospy.get_param("/" + self_name + "_controller")
+    start_dist = rospy.get_param("/" + self_name + "_dist")
+    traj = rospy.get_param("/" + self_name + "_traj")
 
-    if rospy.has_param("/other_name"):
-        other_name = rospy.get_param("/other_name")
+    if rospy.has_param("/" + self_name + "_other_name"):
+        other_name = rospy.get_param("/" + self_name + "_other_name")
     else:
         other_name = None
 
-    if rospy.has_param("/profile"):
-        profile = rospy.get_param("/profile")
+    if rospy.has_param("/" + self_name + "_profile"):
+        profile = rospy.get_param("/" + self_name + "_profile")
     else:
         profile = None
 
-    if rospy.has_param("/left_curb"):
-        left_curb_file = rospy.get_param("/left_curb")
+    if rospy.has_param("/" + self_name + "_left_curb"):
+        left_curb_file = rospy.get_param("/" + self_name + "_left_curb")
     else:
         left_curb_file = ""