From f364b7f95775eee7c4e80326ea8892f6d0b36cf3 Mon Sep 17 00:00:00 2001
From: PatXue <95881915+PatXue@users.noreply.github.com>
Date: Sat, 17 Feb 2024 17:47:56 -0500
Subject: [PATCH] Setup system to get starting pose from start dist

Code still not tested, could not figure out how to import world and trajectory into engine.py
---
 rb_ws/src/buggy/launch/sim_2d_single.launch |  4 +--
 rb_ws/src/buggy/scripts/2d_sim/engine.py    | 28 +++++++++++++++------
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index 1f3e4974..b586f5d3 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/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="starting_dist"    default="0.0" />
     <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 starting_dist) $(arg velocity) $(arg buggy_name)"/>
 
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg autonsystem_args)"/>
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index 388436c2..2999bcea 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -9,6 +9,8 @@
 from nav_msgs.msg import Odometry
 import numpy as np
 import utm
+from buggy import Trajectory
+from buggy.scripts.auton.world import World
 
 
 class Simulator:
@@ -22,7 +24,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 +54,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 +71,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))
+        init_pose = World.world_to_utm_pose(Pose(init_x, init_y, init_heading))
+
+        self.e_utm = init_pose.x
+        self.n_utm = init_pose.y
+        self.heading = init_pose.theta
         self.velocity = velocity # m/s
 
         self.steering_angle = 0  # degrees
@@ -287,9 +297,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()