diff --git a/.gitignore b/.gitignore
index 60a6406..e8edffc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,4 +5,6 @@ rb_ws/bags
docker-compose.yml~
*TEMP_DO_NOT_EDIT.txt
rb_ws/src/buggy/bags/*
-*.bag
\ No newline at end of file
+*.bag
+.vision*/*
+vision/data*/*
\ No newline at end of file
diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py
index ce152c0..f960de7 100644
--- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py
+++ b/rb_ws/src/buggy/buggy/controller/controller_superclass.py
@@ -1,7 +1,6 @@
from abc import ABC, abstractmethod
import sys
-from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Odometry
diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py
index 932538c..3407653 100644
--- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py
+++ b/rb_ws/src/buggy/buggy/controller/stanley_controller.py
@@ -35,7 +35,7 @@ def __init__(self, start_index, namespace, node):
self.debug_error_publisher = self.node.create_publisher(
ROSPose, "auton/debug/error", 1
)
-
+
def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
"""Computes the steering angle determined by Stanley controller.
Does this by looking at the crosstrack error + heading error
@@ -50,7 +50,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
if self.current_traj_index >= trajectory.get_num_points() - 1:
self.node.get_logger.error("[Stanley]: Ran out of path to follow!")
raise Exception("[Stanley]: Ran out of path to follow!")
-
+
current_rospose = state_msg.pose.pose
current_speed = np.sqrt(
state_msg.twist.twist.linear.x**2 + state_msg.twist.twist.linear.y**2
@@ -58,7 +58,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
yaw_rate = state_msg.twist.twist.angular.z
heading = current_rospose.orientation.z
x, y = current_rospose.position.x, current_rospose.position.y #(Easting, Northing)
-
+
front_x = x + StanleyController.WHEELBASE * np.cos(heading)
front_y = y + StanleyController.WHEELBASE * np.sin(heading)
@@ -76,12 +76,12 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
self.LOOK_AHEAD_DIST_CONST * current_speed,
self.MIN_LOOK_AHEAD_DIST,
self.MAX_LOOK_AHEAD_DIST)
-
+
traj_dist = trajectory.get_distance_from_index(self.current_traj_index) + lookahead_dist
ref_heading = trajectory.get_heading_by_index(
trajectory.get_index_from_distance(traj_dist))
-
+
error_heading = ref_heading - heading
error_heading = np.arctan2(np.sin(error_heading), np.cos(error_heading)) #Bounds error_heading
@@ -103,7 +103,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
cross_track_component = -np.arctan2(
StanleyController.CROSS_TRACK_GAIN * error_dist, current_speed + StanleyController.K_SOFT
)
-
+
# Calculate yaw rate error
r_meas = yaw_rate
r_traj = current_speed * (trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist) + 0.05)
@@ -118,7 +118,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
current_pose = Pose(current_rospose.position.x, current_rospose.position.y, heading)
reference_error = current_pose.convert_point_from_global_to_local_frame(closest_position)
reference_error -= np.array([StanleyController.WHEELBASE, 0]) #Translae back to back wheel to get accurate error
-
+
error_pose = ROSPose()
error_pose.position.x = reference_error[0]
error_pose.position.y = reference_error[1]
diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py
index 85b996f..cf9f662 100644
--- a/rb_ws/src/buggy/buggy/util/trajectory.py
+++ b/rb_ws/src/buggy/buggy/util/trajectory.py
@@ -1,7 +1,4 @@
import json
-import time
-import uuid
-import matplotlib.pyplot as plt
# from buggy.msg import TrajectoryMsg
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml
index bc455d6..2c62b1b 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_single.xml
@@ -13,11 +13,11 @@
-
+