From c043e33ab808f80d5df13457020f6d05d3b5071e Mon Sep 17 00:00:00 2001
From: Mehul Goel <mehulgoel873@gmail.com>
Date: Wed, 26 Jun 2024 16:41:25 -0700
Subject: [PATCH] Finished trajectory class

---
 rb_ws/src/buggy/launch/sim_2d_2buggies.launch | 8 ++++----
 rb_ws/src/buggy/msg/TrajectoryMsg.msg         | 1 +
 rb_ws/src/buggy/scripts/auton/path_planner.py | 1 -
 rb_ws/src/buggy/scripts/auton/trajectory.py   | 4 +++-
 4 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
index e737d2a..f94fb30 100755
--- a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,13 +1,13 @@
 <launch>
     <arg name="sc_controller"     default="stanley" />
     <arg name="sc_start_pos"    default="Hill1_SC" />
-    <arg name="sc_velocity"         default="12.0" />
+    <arg name="sc_velocity"         default="15.0" />
 
     <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="10.0" />
+    <arg name="nand_velocity"       default="7.0" />
 
     <arg name="manual_vel"          default="false" />
     <arg name="auto_vel"            default="false" />
@@ -35,11 +35,11 @@
         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 --left_curb buggycourse_curb.json" />
+    <arg name="nand_autonsystem_args" default="--controller $(arg nand_controller) --dist 0.0 --traj $(arg nand_path) --self_name 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 --left_curb buggycourse_curb.json" />
+    <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)"/>
 
diff --git a/rb_ws/src/buggy/msg/TrajectoryMsg.msg b/rb_ws/src/buggy/msg/TrajectoryMsg.msg
index b60abda..6aa4aa9 100644
--- a/rb_ws/src/buggy/msg/TrajectoryMsg.msg
+++ b/rb_ws/src/buggy/msg/TrajectoryMsg.msg
@@ -1,2 +1,3 @@
+float64 time
 float64[] easting
 float64[] northing
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py
index ba94aed..36ec83e 100755
--- a/rb_ws/src/buggy/scripts/auton/path_planner.py
+++ b/rb_ws/src/buggy/scripts/auton/path_planner.py
@@ -206,7 +206,6 @@ def compute_traj(
 
         local_traj = Trajectory(json_filepath=None, positions=positions)
         self.traj_publisher.publish(local_traj.pack())
-        print("published???")
         return local_traj, \
                 local_traj.get_closest_index_on_path(
                 self_pose.x,
diff --git a/rb_ws/src/buggy/scripts/auton/trajectory.py b/rb_ws/src/buggy/scripts/auton/trajectory.py
index 14a735c..2106625 100755
--- a/rb_ws/src/buggy/scripts/auton/trajectory.py
+++ b/rb_ws/src/buggy/scripts/auton/trajectory.py
@@ -5,6 +5,7 @@
 
 from scipy.interpolate import Akima1DInterpolator, CubicSpline
 from buggy.msg import TrajectoryMsg
+import time
 
 from world import World
 
@@ -352,10 +353,11 @@ def pack(self):
         traj = TrajectoryMsg()
         traj.easting = self.positions[:, 0]
         traj.northing = self.positions[:, 1]
+        traj.time = time.time()
         return traj
     
     def unpack(trajMsg : TrajectoryMsg):
-        pos = np.array([trajMsg.easting, trajMsg.northing])
+        pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0)
         return Trajectory(positions=pos)