Finished trajectory class

CMU-Robotics-Club · Jun 26, 2024 · c043e33 · c043e33
1 parent 576bcc4
commit c043e33
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 6 deletions.
diff --git 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
@@ -1,2 +1,3 @@
+float64 time
 float64[] easting
 float64[] northing
diff --git 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
@@ -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)