diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py index 4b3a200..e0b11b3 100755 --- a/rb_ws/src/buggy/scripts/auton/path_planner.py +++ b/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) \ No newline at end of file + self.traj_publisher.publish(local_traj.pack(self_pose.x, self_pose.y)) \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/trajectory.py b/rb_ws/src/buggy/scripts/auton/trajectory.py index adab5c7..834fd38 100755 --- a/rb_ws/src/buggy/scripts/auton/trajectory.py +++ b/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__":