cleanup

rb_ws/src/buggy/scripts/2d_sim/controller_2d.py

@@ -1,9 +1,8 @@
 #! /usr/bin/env python3
 import sys
+import threading
 import rospy
 from std_msgs.msg import Float64
-import threading
-import numpy as np
 
 class Controller:
     def __init__(self, buggy_name):
@@ -15,7 +14,6 @@ def __init__(self, buggy_name):
         self.lock = threading.Lock()
         self.set_steering(0)
         self.set_velocity(0)
-
     def set_steering(self, angle: float):
         """Set the steering angle and publish to simulator engine
 
@@ -27,9 +25,7 @@ def set_steering(self, angle: float):
 
         with self.lock:
             self.steering_angle = angle
-
         self.steering_publisher.publish(msg)
-
     def set_velocity(self, vel: float):
         """Set the velocity and publish to simulator engine
 
@@ -41,7 +37,6 @@ def set_velocity(self, vel: float):
 
         with self.lock:
             self.velocity = vel
-
         self.velocity_publisher.publish(msg)
 
 if __name__ == "__main__":

rb_ws/src/buggy/scripts/2d_sim/engine.py

@@ -1,14 +1,13 @@
 #! /usr/bin/env python3
-import rospy
 from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance
 from std_msgs.msg import Float64
 from sensor_msgs.msg import NavSatFix
 from nav_msgs.msg import Odometry
 import threading
 import numpy as np
 import utm
-import time
 import sys
+import rospy
 
 
 class Simulator:
@@ -40,7 +39,6 @@ def __init__(self, starting_pose, velocity, buggy_name):
         self.velocity_subscriber = rospy.Subscriber(
             buggy_name + "/velocity", Float64, self.update_velocity
         ) 
-
         # to plot on Foxglove (no noise)
         self.navsatfix_publisher = rospy.Publisher(
             buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1
@@ -50,7 +48,6 @@ def __init__(self, starting_pose, velocity, buggy_name):
         self.navsatfix_noisy_publisher = rospy.Publisher(
             buggy_name + "/state/pose_navsat_noisy", NavSatFix, queue_size=1
         )
-
         # (UTM east, UTM north, HEADING(degs))
         self.starting_poses = {
             "Hill1_SC": (Simulator.UTM_EAST_ZERO + 60, Simulator.UTM_NORTH_ZERO + 150, -110),
@@ -69,7 +66,6 @@ def __init__(self, starting_pose, velocity, buggy_name):
         # utm_coords = utm.from_latlon(Simulator.START_LAT, Simulator.START_LONG)
         # self.e_utm = utm_coords[0]
         # self.n_utm = utm_coords[1]
-
         self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose]        
         self.velocity = velocity # m/s
 
@@ -96,9 +92,7 @@ def update_velocity(self, data: Float64):
             source (string): whether incoming data is manual or simulated
         """
         with self.lock:
-            self.velocity = data.data
-
-
+            self.velocity = data.data  
     def get_steering_arc(self):
         # Adapted from simulator.py (Joseph Li)
         # calculate the radius of the steering arc
@@ -112,7 +106,6 @@ def get_steering_arc(self):
             return np.inf
 
         return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle))
-
     def dynamics(self, state, v):
         """ Calculates continuous time bicycle dynamics as a function of state and velocity
 
@@ -139,7 +132,6 @@ def step(self):
             n_utm = self.n_utm
             velocity = self.velocity
             steering_angle = self.steering_angle
-
         # Calculate new position
         if steering_angle == 0.0:
             # Straight
@@ -149,9 +141,7 @@ def step(self):
         else:
             # steering radius
             radius = self.get_steering_arc()
-
             distance = velocity / self.rate
-
             delta_heading = distance / radius
             heading_new = heading + np.rad2deg(delta_heading) / 2
             e_utm_new = e_utm + (velocity / self.rate) * np.cos(np.deg2rad(heading_new))
@@ -162,12 +152,10 @@ def step(self):
             self.e_utm = e_utm_new
             self.n_utm = n_utm_new
             self.heading = heading_new
-
     def publish(self):
         """Publishes the pose the arrow in visualizer should be at"""
         p = Pose()
         time_stamp = rospy.Time.now()
-
         with self.lock:
             p.position.x = self.e_utm
             p.position.y = self.n_utm

rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py

@@ -1,20 +1,15 @@
 #! /usr/bin/env python3
-import numpy as np
 import rospy
 import sys
 from controller_2d import Controller
-from std_msgs.msg import Float64
 import threading
-import math
 import tkinter as tk
-import keyboard
 
 class VelocityUI:
 
 
     def __init__(self, init_vel: float, buggy_name: str):
         self.buggy_vel = 0 # So the buggy doesn't start moving without user input
-
         self.controller = Controller(buggy_name)
         self.lock = threading.Lock()
 
@@ -30,9 +25,6 @@ def __init__(self, init_vel: float, buggy_name: str):
         self.root.bind("<Up>", lambda i: self.scale.set(self.scale.get() + 2))
         self.root.bind("<Down>", lambda d: self.scale.set(self.scale.get() - 2))
 
-
-
-
     def step(self):
         """sets velocity of buggy to the current scale value 
             called once every tick
@@ -43,11 +35,8 @@ def step(self):
         self.buggy_vel = self.scale.get()/10 # so we can set velocity with 0.1 precision
         self.controller.set_velocity(self.buggy_vel)
 
-
-
 if __name__ == "__main__":
     rospy.init_node("velocity_ui")
-
     init_vel = float(sys.argv[1])
     buggy_name = sys.argv[2]
     vel = VelocityUI(init_vel, buggy_name)

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

@@ -1,6 +1,5 @@
 #!/usr/bin/env python3
 
-import rospy
 import sys
 
 # ROS Message Imports
@@ -9,6 +8,8 @@
 
 import numpy as np
 from threading import Lock
+import rospy
+
 
 from trajectory import Trajectory
 from world import World
@@ -35,24 +36,19 @@ class AutonSystem:
     controller: Controller = None
     brake_controller: BrakeController = None
     lock = None
-
     steer_publisher = None
-
     ticks = 0
 
     def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim) -> None:
         self.trajectory = trajectory
         self.controller = controller
         self.brake_controller = brake_controller
-
         self.lock = Lock()
         self.ticks = 0
         self.msg = None
-
         if (is_sim):
             rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg)
 
-
         rospy.Subscriber(buggy_name + "nav/odom", Odometry, self.update_msg)
 
         self.covariance_warning_publisher = rospy.Publisher(
@@ -90,7 +86,6 @@ def update_msg(self, msg):
     def tick_caller(self):
         while ((not rospy.is_shutdown()) and (self.msg == None)): # with no message, we wait
             rospy.sleep(0.001)
-
         # wait for covariance matrix to be better
         while ((not rospy.is_shutdown()) and
                (self.msg.pose.covariance[0] ** 2 + self.msg.pose.covariance[7] ** 2 > 1**2)):
@@ -177,7 +172,6 @@ def tick(self):
         ctrller = ModelPredictiveController(buggy_name, start_index)
     if (ctrller == None):
         raise Exception("Invalid Controller Argument")
-
     auton_system = AutonSystem(
         trajectory,
         ctrller,