diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py index 4ca18a61..2caa89dd 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py +++ b/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__": diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index f5cffb65..bc596876 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -1,5 +1,4 @@ #! /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 @@ -7,8 +6,8 @@ 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 diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py index 83357f7b..4444c71b 100644 --- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py +++ b/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("", lambda i: self.scale.set(self.scale.get() + 2)) self.root.bind("", 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) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index e8d783c0..3931850d 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/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,