Sw/UI velocity

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,4 +1,5 @@
 #! /usr/bin/env python3
+import sys
 import rospy
 from std_msgs.msg import Float64
 import threading
@@ -50,5 +51,6 @@ def set_velocity(self, vel: float):
     rate = rospy.Rate(5)
     i = 0
     while not rospy.is_shutdown():
+        controller.set_velocity(15)
         rate.sleep()
 

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

@@ -20,7 +20,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,9 +36,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(
@@ -92,10 +93,12 @@ 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
@@ -130,13 +133,13 @@ def dynamics(self, state, v):
 
     def step(self):
         """Complete the step at a certain Hz here. Do physics"""
-        with self.lock:
+        with self.lock: 
             heading = self.heading
             e_utm = self.e_utm
             n_utm = self.n_utm
             velocity = self.velocity
             steering_angle = self.steering_angle
-
+        
         # Calculate new position
         if steering_angle == 0.0:
             # Straight
@@ -300,3 +303,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,54 @@
+#! /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()
+
+        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):
+        self.root.update_idletasks()
+        self.root.update()
+        '''Update velocity of buggy'''
+        self.buggy_vel = self.scale.get()/10
+        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

@@ -53,6 +53,7 @@ def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim)
             rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg)
 
 
+        rospy.Subscriber("nav/odom", Odometry, self.update_msg)
         self.covariance_warning_publisher = rospy.Publisher(
             buggy_name + "/debug/is_high_covariance", Bool, queue_size=1
         )