Sw/UI velocity (#38)

* Create velocity_updater.py

Basic framework of VelocityUpdater class, just applies constant acceleration to the buggy

* updated engine, keyboard import erroring

* added publisher/subscriber for manual velocity (can publish via foxglove node)

* fixed python reqs

* cleaned up comments

* integrated manual controller with simulated control

* removed print debugs + bound scale to arrow keys

* reversed binding

* final cleanup


---------

Co-authored-by: PatXue <[email protected]>
Co-authored-by: TiaSinghania <[email protected]>

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

rb_ws/src/buggy/launch/sim_2d_2buggies.launch

@@ -21,6 +21,8 @@
         args="$(arg sc_starting_pose) $(arg sc_velocity) SC"/>
     <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
         output="screen" args="$(arg sc_velocity) SC"/>
+    <node name="sc_velocity_ui" pkg="buggy" type="velocity_ui.py"
+        output="screen" args="$(arg sc_velocity) SC"/>
 
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
@@ -30,6 +32,8 @@
         args="$(arg nand_starting_pose) $(arg nand_velocity) Nand"/>
     <node name="nand_vel_updater" pkg="buggy" type="velocity_updater.py"
         output="screen" args="$(arg nand_velocity) Nand"/>
+    <node name="nand_velocity_ui" pkg="buggy" type="velocity_ui.py"
+        output="screen" args="$(arg nand_velocity) Nand"/>
 
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg nand_controller) $(arg nand_start_dist) $(arg nand_path) Nand True"/>

rb_ws/src/buggy/launch/sim_2d_single.launch

@@ -20,4 +20,4 @@
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
         args="$(arg controller) $(arg start_dist) $(arg path) $(arg buggy_name) True"/>
 
-</launch>
+</launch>

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__":

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

@@ -1,14 +1,13 @@
 #! /usr/bin/env python3
-import rospy
+import sys
+import threading
 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:
@@ -20,7 +19,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):
         """
@@ -36,10 +35,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
@@ -49,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),
@@ -68,8 +66,7 @@ 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.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose]
         self.velocity = velocity # m/s
 
         self.steering_angle = 0  # degrees
@@ -92,10 +89,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
@@ -109,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
 
@@ -121,7 +117,7 @@ def dynamics(self, state, v):
             dstate (np.Array): time derivative of state from dynamics
         """
         l = Simulator.WHEELBASE
-        x, y, theta, delta = state
+        _, _, theta, delta = state
 
         return np.array([v * np.cos(theta),
                          v * np.sin(theta),
@@ -136,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
@@ -146,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))
@@ -159,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
@@ -300,3 +291,4 @@ def loop(self):
     buggy_name = sys.argv[3]
     sim = Simulator(starting_pose, velocity, buggy_name)
     sim.loop()
+

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('Manual Velocity')
+        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("<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
+        """
+        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()

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

@@ -1,15 +1,15 @@
 #!/usr/bin/env python3
 
-import rospy
 import sys
+from threading import Lock
+
+import numpy as np
+import rospy
 
 # ROS Message Imports
 from std_msgs.msg import Float32, Float64, Bool
 from nav_msgs.msg import Odometry
 
-import numpy as np
-from threading import Lock
-
 from trajectory import Trajectory
 from world import World
 from controller import Controller
@@ -35,24 +35,20 @@ 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(
             buggy_name + "/debug/is_high_covariance", Bool, queue_size=1
         )
@@ -84,11 +80,10 @@ def update_speed(self, msg):
     def update_msg(self, msg):
         with self.lock:
             self.msg = 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)):
@@ -175,7 +170,6 @@ def tick(self):
         ctrller = ModelPredictiveController(buggy_name, start_index)
     if (ctrller == None):
         raise Exception("Invalid Controller Argument")
-
     auton_system = AutonSystem(
         trajectory,
         ctrller,