diff --git a/.gitignore b/.gitignore
index 60a64061..e8edffcc 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_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py
index ee1bb2fe..700bef06 100644
--- a/rb_ws/src/buggy/buggy/controller/controller_node.py
+++ b/rb_ws/src/buggy/buggy/controller/controller_node.py
@@ -25,7 +25,7 @@ def __init__(self):
"""
super().__init__('controller')
self.get_logger().info('INITIALIZED.')
-
+
#Parameters
self.declare_parameter("dist", 0.0) #Starting Distance along path
@@ -38,12 +38,14 @@ def __init__(self):
start_index = self.cur_traj.get_index_from_distance(start_dist)
self.declare_parameter("controller_name", "stanley")
- match self.get_parameter("controller_name").value:
- case "stanley":
- self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY
- case _:
- self.get_logger().error("Invalid Controller Name!")
- raise Exception("Invalid Controller Argument")
+
+ controller_name = self.get_parameter("controller_name").value
+ print(controller_name.lower)
+ if (controller_name.lower() == "stanley"):
+ self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY
+ else:
+ self.get_logger().error("Invalid Controller Name: " + controller_name.lower())
+ raise Exception("Invalid Controller Argument")
# Publishers
self.init_check_publisher = self.create_publisher(Bool,
@@ -67,7 +69,7 @@ def __init__(self):
timer_period = 0.01 # seconds (100 Hz)
self.timer = self.create_timer(timer_period, self.loop)
-
+
def odom_listener(self, msg : Odometry):
'''
This is the subscriber that updates the buggies state for navigation
@@ -75,7 +77,7 @@ def odom_listener(self, msg : Odometry):
'''
with self.lock:
self.odom = msg
-
+
def traj_listener(self, msg): #TYPE UNKOWN AS OF NOW?? CUSTOM TYPE WHEN #TODO: FIGURE OUT BEFORE MERGE
'''
This is the subscriber that updates the buggies trajectory for navigation
@@ -97,11 +99,11 @@ def init_check(self):
if (self.odom == None):
self.get_logger().warn("WARNING: no available position estimate")
return False
-
+
elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1**2):
self.get_logger().warn("checking position estimate certainty")
return False
-
+
#Originally under a lock, doesn't seem necessary?
current_heading = self.odom.pose.pose.orientation.z
closest_heading = self.cur_traj.get_heading_by_index(self.cur_traj.get_closest_index_on_path(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y))
@@ -116,11 +118,11 @@ def init_check(self):
current_heading = 2 * np.pi + current_heading
if (closest_heading < 0):
closest_heading = 2 * np.pi + closest_heading
-
+
if (abs(current_heading - closest_heading) >= np.pi/2):
self.get_logger().warn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(current_heading)))
return False
-
+
return True
def loop(self):
@@ -133,13 +135,13 @@ def loop(self):
self.get_logger().info("Passed Initialization Check")
else:
return
-
+
self.heading_publisher.publish(Float32(data=np.rad2deg(self.odom.pose.pose.orientation.z)))
steering_angle = self.controller.compute_control(self.odom, self.cur_traj)
steering_angle_deg = np.rad2deg(steering_angle)
self.steer_publisher.publish(Float64(data=float(steering_angle_deg)))
-
+
def main(args=None):
rclpy.init(args=args)
diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py
index ce152c07..f960de75 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 932538c2..34076535 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 85b996f2..cf9f662c 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 bc455d62..2c62b1b1 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 @@
-
+