diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 17ebb09..57b8fbc 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -124,7 +124,7 @@ def update_other_odom(self, msg): def update_other_steering_angle(self, msg): with self.lock: self.other_steering = msg.data - + def update_traj(self, msg): with self.lock: # print("New Trajectory:") diff --git a/rb_ws/src/buggy/scripts/auton/trajectory.py b/rb_ws/src/buggy/scripts/auton/trajectory.py index ddb924f..ea1d59d 100755 --- a/rb_ws/src/buggy/scripts/auton/trajectory.py +++ b/rb_ws/src/buggy/scripts/auton/trajectory.py @@ -1,11 +1,12 @@ import json +import time import uuid import matplotlib.pyplot as plt -import numpy as np -from scipy.interpolate import Akima1DInterpolator, CubicSpline from buggy.msg import TrajectoryMsg -import time + +import numpy as np +from scipy.interpolate import Akima1DInterpolator, CubicSpline from world import World @@ -355,7 +356,7 @@ def pack(self) -> TrajectoryMsg: traj.northing = self.positions[:, 1] traj.time = time.time() return traj - + def unpack(trajMsg : TrajectoryMsg) -> Trajectory: pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0) return Trajectory(positions=pos)