Sim launch position arg

rb_ws/src/buggy/launch/sim_2d_2buggies.launch

@@ -1,12 +1,12 @@
 <launch>
-    <arg name="sc_starting_pose"    default="Hill1_SC" />
+    <arg name="sc_start_dist"       default="0.0" />
     <arg name="sc_velocity"         default="15.0" />
 
     <arg name="nand_controller"     default="stanley" />
     <arg name="nand_path"           default="buggycourse_safe_1.json" />
-
-    <arg name="nand_starting_pose"  default="Hill1_NAND" />
+    <arg name="nand_start_dist"     default="36.0" />
     <arg name="nand_velocity"       default="10.0" />
+
     <arg name="manual_vel"          default="true" />
     <arg name="auto_vel"            default="false" />
 
@@ -30,17 +30,17 @@
 
 
     <node name="nand_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg nand_starting_pose) $(arg nand_velocity) NAND"/>
+        args="$(arg nand_start_dist) $(arg nand_velocity) NAND"/>
 
     <!-- NAND is not aware of SC -->
-    <arg name="nand_autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name NAND" />
+    <arg name="nand_autonsystem_args" default="--controller stanley --dist $(arg nand_start_dist) --traj buggycourse_safe_1.json --self_name NAND" />
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg nand_autonsystem_args)"/>
 
     <node name="sc_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg sc_starting_pose) $(arg sc_velocity) SC"/>
+        args="$(arg sc_start_dist) $(arg sc_velocity) SC"/>
 
-    <arg name="sc_autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name SC --other_name NAND" />
+    <arg name="sc_autonsystem_args" default="--controller stanley --dist $(arg sc_start_dist) --traj buggycourse_safe_1.json --self_name SC --other_name NAND" />
     <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg sc_autonsystem_args)"/>
 

rb_ws/src/buggy/launch/sim_2d_single.launch

@@ -1,6 +1,6 @@
 <launch>
-    <arg name="autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name SC" />
-    <arg name="starting_pose"    default="Hill1_SC" />
+    <arg name="start_dist"       default="0.0" />
+    <arg name="autonsystem_args" default="--controller stanley --dist $(arg start_dist) --traj buggycourse_safe_1.json --self_name SC" />
     <arg name="velocity"         default="15.0" />
     <arg name="buggy_name"       default="SC" />
 
@@ -22,7 +22,7 @@
     </group>
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
+        args="$(arg start_dist) $(arg velocity) $(arg buggy_name)"/>
 
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg autonsystem_args)"/>

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

@@ -9,6 +9,9 @@
 from nav_msgs.msg import Odometry
 import numpy as np
 import utm
+sys.path.append("/rb_ws/src/buggy/scripts/auton")
+from trajectory import Trajectory
+from world import World
 
 
 class Simulator:
@@ -22,7 +25,7 @@ class Simulator:
     START_LONG = -79.9409643423245
     NOISE = True # Noisy outputs for nav/odom?
 
-    def __init__(self, starting_pose, velocity, buggy_name):
+    def __init__(self, starting_dist, velocity, buggy_name):
         """
         Args:
             heading (float): degrees start heading of buggy
@@ -52,10 +55,10 @@ def __init__(self, starting_pose, velocity, buggy_name):
             )
 
         # (UTM east, UTM north, HEADING(degs))
-        self.starting_poses = {
-            "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110),
-            "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110),
-        }
+        # self.starting_poses = {
+        #     "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110),
+        #     "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110),
+        # }
 
         # Start position for End of Hill 2
         # self.e_utm = Simulator.UTM_EAST_ZERO - 3
@@ -69,7 +72,15 @@ def __init__(self, starting_pose, velocity, buggy_name):
         # utm_coords = utm.from_latlon(Simulator.START_LAT, Simulator.START_LONG)
         # self.e_utm = utm_coords[0]
         # self.n_utm = utm_coords[1]
-        self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose]
+
+        trajectory = Trajectory("/rb_ws/src/buggy/paths/buggycourse_safe_1.json")
+        init_x, init_y = trajectory.get_position_by_distance(starting_dist)
+        init_heading = np.rad2deg(trajectory.get_heading_by_distance(starting_dist)[0])
+        init_pose = World.world_to_utm_numpy(np.array([init_x, init_y]).reshape((1,2)))
+
+        self.e_utm = init_pose[0,0]
+        self.n_utm = init_pose[0,1]
+        self.heading = init_heading
         self.velocity = velocity # m/s
 
         self.steering_angle = 0  # degrees
@@ -96,6 +107,7 @@ def update_velocity(self, data: Float64):
         """
         with self.lock:
             self.velocity = data.data
+
     def get_steering_arc(self):
         # Adapted from simulator.py (Joseph Li)
         # calculate the radius of the steering arc
@@ -109,6 +121,7 @@ def get_steering_arc(self):
             return np.inf
 
         return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle))
+
     def dynamics(self, state, v):
         """ Calculates continuous time bicycle dynamics as a function of state and velocity
 
@@ -287,9 +300,11 @@ def loop(self):
     rospy.init_node("sim_2d_engine")
     print("sim 2d eng args:")
     print(sys.argv)
-    starting_pose = sys.argv[1]
+
+    starting_dist = float(sys.argv[1])
     velocity = float(sys.argv[2])
     buggy_name = sys.argv[3]
-    sim = Simulator(starting_pose, velocity, buggy_name)
+
+    sim = Simulator(starting_dist, velocity, buggy_name)
     sim.loop()