updated path planner node

Author: Jackack

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

@@ -87,7 +87,7 @@ def compute_traj(
         self,
         self_pose: Pose,
         other_pose: Pose, #Currently NAND's location -- To be Changed
-        ) -> Trajectory:
+        ) -> None:
         """
         draw trajectory starting at the current pose and ending at a fixed distance
         ahead. For each trajectory point, calculate the required offset perpendicular to the nominal
@@ -193,9 +193,6 @@ def compute_traj(
         )
         positions = nominal_slice + (passing_offsets[:, None] * nominal_normals)
 
-        # prepend current pose
-        # positions = np.vstack((np.array([self_pose.x, self_pose.y]), positions))
-
         # publish passing targets for debugging
         for i in range(len(positions)):
             reference_navsat = NavSatFix()
@@ -205,8 +202,4 @@ def compute_traj(
             self.debug_passing_traj_publisher.publish(reference_navsat)
 
         local_traj = Trajectory(json_filepath=None, positions=positions)
-        self.traj_publisher.publish(local_traj.pack(self_pose.x, self_pose.y))
-        return local_traj, \
-                local_traj.get_closest_index_on_path(
-                self_pose.x,
-                self_pose.y)
+        self.traj_publisher.publish(local_traj.pack(self_pose.x, self_pose.y))

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

@@ -361,7 +361,7 @@ def pack(self, x, y) -> TrajectoryMsg:
     def unpack(trajMsg : TrajectoryMsg):
         pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0)
         cur_idx = trajMsg.cur_idx
-        return Trajectory(positions=pos), cur_idx
+        return Trajectory(json_filepath=None, positions=pos), cur_idx
 
 
 if __name__ == "__main__":