fixed merge conflicts with main

CMU-Robotics-Club · Feb 14, 2024 · 1f3b933 · 1f3b933
2 parents 1939d47 + 65677d0
commit 1f3b933
Show file tree

Hide file tree

Showing 8 changed files with 96 additions and 47 deletions.
diff --git 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
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,37 +1,47 @@
-<!-- roslaunch buggy main.launch simulation:=true -->
-<!-- roslaunch buggy main.launch simulation:=false -->
-<!-- <arg name="name" value="INS" /> -->
-
 <launch>
     <arg name="sc_starting_pose"    default="Hill1_SC" />
     <arg name="sc_velocity"         default="15.0" />
-    
+
     <arg name="nand_controller"     default="stanley" />
     <arg name="nand_path"           default="buggycourse_safe_1.json" />
 
     <arg name="nand_starting_pose"  default="Hill1_NAND" />
     <arg name="nand_velocity"       default="10.0" />
+    <arg name="manual_vel"          default="true" />
+    <arg name="auto_vel"            default="false" />
+
+    <group if="$(arg auto_vel)">
+        <!-- Run the auto velocity updater -->
+        <node name="nand_vel_updater" pkg="buggy" type="velocity_updater.py"
+            output="screen" args="$(arg nand_velocity) NAND"/>
+        <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
+            output="screen" args="$(arg sc_velocity) SC"/>
+    </group>
+
+    <group if="$(arg manual_vel)">
+        <!-- Run the manual velocity updater -->
+        <node name="sc_velocity_ui" pkg="buggy" type="velocity_ui.py"
+            output="screen" args="$(arg sc_velocity) SC"/>
+        <node name="nand_velocity_ui" pkg="buggy" type="velocity_ui.py"
+            output="screen" args="$(arg nand_velocity) NAND"/>
+    </group>
+
+    <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
+
 
-
-    <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />  
-
-    <node name="nand_sim_2d_engine" pkg="buggy" type="engine.py" output="screen" 
+    <node name="nand_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         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"/> -->
 
     <!-- NAND is not aware of SC -->
     <arg name="nand_autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name NAND" />
-    <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
+    <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg nand_autonsystem_args)"/>
 
     <node name="sc_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         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"/> -->
 
     <arg name="sc_autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name SC --other_name NAND" />
-    <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
+    <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg sc_autonsystem_args)"/>
 
 

diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -1,7 +1,3 @@
-<!-- roslaunch buggy main.launch simulation:=true -->
-<!-- roslaunch buggy main.launch simulation:=false -->
-<!-- <arg name="name" value="INS" /> -->
-
 <launch>
     <arg name="autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name SC" />
     <arg name="starting_pose"    default="Hill1_SC" />
@@ -10,13 +6,25 @@
 
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
 
+    <arg name="manual_vel"          default="true" />
+    <arg name="auto_vel"            default="false" />
+
+    <group if="$(arg auto_vel)">
+        <!-- Run the auto velocity updater -->
+        <node name="sc_vel_updater" pkg="buggy" type="velocity_updater.py"
+            output="screen" args="$(arg velocity) SC"/>
+    </group>
+
+    <group if="$(arg manual_vel)">
+        <!-- Run the manual velocity updater -->
+        <node name="sc_velocity_ui" pkg="buggy" type="velocity_ui.py"
+            output="screen" args="$(arg velocity) SC"/>
+    </group>
+
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
 
-    <!-- <node name="vel_updater" pkg="buggy" type="velocity_updater.py"
-        output="screen" args="$(arg velocity) $(arg buggy_name)"/> -->
-
-    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
+    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg autonsystem_args)"/>
 
-</launch>
+</launch>
diff --git 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
@@ -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
@@ -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("<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()
diff --git 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
@@ -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)