diff --git a/python-requirements.txt b/python-requirements.txt
index cbc5d7de..254c5dac 100755
--- a/python-requirements.txt
+++ b/python-requirements.txt
@@ -8,3 +8,5 @@ pymap3d==3.0.1
scipy==1.10.1
trimesh==3.23.5
utm==0.7.0
+keyboard==0.13.5
+tk==0.1.0
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
index 302e61db..2cda83d1 100755
--- a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,37 +1,47 @@
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
index d1a6cd9a..1f3e4974 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -1,7 +1,3 @@
-
-
-
-
@@ -10,13 +6,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
\ No newline at end of file
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 9598c485..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,8 +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):
@@ -14,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
@@ -26,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
@@ -40,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 aec5620e..613f11c8 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -12,7 +12,6 @@
import utm
-
class Simulator:
UTM_EAST_ZERO = 589702.87
UTM_NORTH_ZERO = 4477172.947
@@ -22,7 +21,7 @@ class Simulator:
# Start positions for Outdoor track
START_LAT = 40.443024364623916
START_LONG = -79.9409643423245
- NOISE = True # Noisy outputs for nav/odom?
+ NOISE = True # Noisy outputs for nav/odom?
def __init__(self, starting_pose, velocity, buggy_name):
"""
@@ -38,10 +37,10 @@ def __init__(self, starting_pose, velocity, buggy_name):
self.steering_subscriber = rospy.Subscriber(
buggy_name + "/input/steering", Float64, self.update_steering_angle
)
+ # To read from velocity
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
@@ -72,7 +71,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,10 +94,10 @@ def update_velocity(self, data: Float64):
Args:
data (Float64): velocity in m/s
+ source (string): whether incoming data is manual or simulated
"""
with self.lock:
self.velocity = data.data
-
def get_steering_arc(self):
# Adapted from simulator.py (Joseph Li)
# calculate the radius of the steering arc
@@ -113,7 +111,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
@@ -140,7 +137,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
@@ -150,9 +146,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))
@@ -163,12 +157,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
@@ -304,3 +296,4 @@ def loop(self):
buggy_name = sys.argv[3]
sim = Simulator(starting_pose, velocity, buggy_name)
sim.loop()
+
diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
new file mode 100755
index 00000000..09840e3a
--- /dev/null
+++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -0,0 +1,44 @@
+#! /usr/bin/env python3
+import sys
+import threading
+import tkinter as tk
+from controller_2d import Controller
+import rospy
+
+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()
+
+ self.root = tk.Tk()
+
+ self.root.title(buggy_name + ' Manual Velocity: scale = 0.1m/s')
+ self.root.geometry('600x100')
+ self.root.configure(background = '#342d66')
+
+ self.scale = tk.Scale(self.root, from_=0, to=300, orient=tk.HORIZONTAL, length=500, width=30)
+ self.scale.pack()
+
+ 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
+ """
+ self.root.update_idletasks()
+ self.root.update()
+ '''Update velocity of buggy'''
+ 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)
+ rate = rospy.Rate(100)
+ while not rospy.is_shutdown():
+ vel.step()
+ rate.sleep()
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index 0a0af67d..f5bc01e7 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -39,7 +39,6 @@ class AutonSystem:
local_controller: Controller = None
brake_controller: BrakeController = None
lock = None
-
steer_publisher = None
ticks = 0
@@ -77,7 +76,7 @@ def __init__(self,
other_name + "/buggy/input/steering", Float64, self.update_other_steering_angle
)
-
+ rospy.Subscriber(self_name + "nav/odom", Odometry, self.update_self_odom)
self.covariance_warning_publisher = rospy.Publisher(
self_name + "/debug/is_high_covariance", Bool, queue_size=1
)
@@ -121,7 +120,6 @@ def tick_caller(self):
(self.self_odom_msg == None or
(self.has_other_buggy and self.other_odom_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.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2)):
@@ -160,7 +158,6 @@ def tick_caller(self):
self.local_controller_tick()
-
self.ticks += 1
if self.ticks >= 10:
@@ -275,7 +272,6 @@ def planner_tick(self):
if (local_ctrller == None):
raise Exception("Invalid Controller Argument")
-
auton_system = AutonSystem(
trajectory,
local_ctrller,
diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py
index 852d5ffd..28816582 100755
--- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py
+++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py
@@ -21,7 +21,7 @@ class StanleyController(Controller):
MAX_LOOK_AHEAD_DIST = 2
CROSS_TRACK_GAIN = 1
- HEADING_GAIN = 0.75
+ HEADING_GAIN = 0.3
def __init__(self, buggy_name, start_index=0) -> None:
super(StanleyController, self).__init__(start_index, buggy_name)