Auton overtake feature, as well as pylint fixes (#39)

* updated path planner and autonsystem, added option for building trajectory from __init__ parameter of trajectory.py * [BETA] obstacle avoidance * added halfplane constraints to MPC * Switched path planning to use offsets along the path normal * Cleaned up unused code, added comments * CI (#19) * pylint fixes more pylint fixes pylint fixes for 2d sim and other existing modules pylint fixes yet again pylint fixes yet again #2 * removed ROSbags * added changes requested * Fixed merge issues Added exec permission to velocity_ui.py * Updated velocity ui window title added if statement for selecting velocity source in launch config --------- Co-authored-by: Christian Luu <[email protected]>
CMU-Robotics-Club · Feb 14, 2024 · 65677d0 · 65677d0
1 parent 061e952
commit 65677d0
Show file tree

Hide file tree

Showing 14 changed files with 674 additions and 253 deletions.
diff --git a/rb_ws/src/buggy/launch/main.launch b/rb_ws/src/buggy/launch/main.launch
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,40 +1,48 @@
-<!-- roslaunch buggy main.launch simulation:=true -->
-<!-- roslaunch buggy main.launch simulation:=false -->
-<!-- <arg name="name" value="INS" /> -->
-
 <launch>
-    <arg name="sc_controller"       default="stanley" />
-    <arg name="sc_start_dist"       default="0.0" />
-    <arg name="sc_path"             default="buggycourse_safe_1.json" />
     <arg name="sc_starting_pose"    default="Hill1_SC" />
     <arg name="sc_velocity"         default="15.0" />
-    
+
     <arg name="nand_controller"     default="stanley" />
-    <arg name="nand_start_dist"     default="30.0" />
     <arg name="nand_path"           default="buggycourse_safe_1.json" />
+
     <arg name="nand_starting_pose"  default="Hill1_NAND" />
-    <arg name="nand_velocity"       default="12.0" />
-
+    <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="nand_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
+        args="$(arg nand_starting_pose) $(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"
+        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"/>
-    <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" 
-        args="$(arg sc_controller) $(arg sc_start_dist) $(arg sc_path) SC True"/>
-
-    <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"/>
-    <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"/>
+
+    <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"
+        args="$(arg sc_autonsystem_args)"/>
+
+
 </launch>
diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -1,23 +1,30 @@
-<!-- roslaunch buggy main.launch simulation:=true -->
-<!-- roslaunch buggy main.launch simulation:=false -->
-<!-- <arg name="name" value="INS" /> -->
-
 <launch>
-    <arg name="buggy_name"       default="SC" />
-    <arg name="controller"       default="stanley" />
-    <arg name="start_dist"       default="0.0" />
-    <arg name="path"             default="buggycourse_safe_1.json" />
+    <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" />
     <arg name="velocity"         default="15.0" />
-
+    <arg name="buggy_name"       default="SC" />
+
     <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)"/>
-    <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
-    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" 
-        args="$(arg controller) $(arg start_dist) $(arg path) $(arg buggy_name) True"/>
+
+    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen"
+        args="$(arg autonsystem_args)"/>
 
 </launch>
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -1,13 +1,15 @@
 #! /usr/bin/env python3
+
 import sys
 import threading
+
+import rospy
 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 numpy as np
 import utm
-import rospy
 
 
 class Simulator:
@@ -44,14 +46,16 @@ def __init__(self, starting_pose, velocity, buggy_name):
             buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1
         )
 
-        # to plot on Foxglove (with noise)
-        self.navsatfix_noisy_publisher = rospy.Publisher(
-            buggy_name + "/state/pose_navsat_noisy", NavSatFix, queue_size=1
-        )
+        if Simulator.NOISE:
+            # to plot on Foxglove (with noise)
+            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),
-            "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 55, Simulator.UTM_NORTH_ZERO + 165, -125),
+            "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110),
+            "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110),
         }
 
         # Start position for End of Hill 2
@@ -70,8 +74,8 @@ def __init__(self, starting_pose, velocity, buggy_name):
         self.velocity = velocity # m/s
 
         self.steering_angle = 0  # degrees
-        self.rate = 100  # Hz
-        self.pub_skip = 10  # publish every pub_skip ticks
+        self.rate = 200  # Hz
+        self.pub_skip = 1  # publish every pub_skip ticks
 
         self.lock = threading.Lock()
 

diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py
@@ -13,7 +13,7 @@ def __init__(self, init_vel: float, buggy_name: str):
 
         self.root = tk.Tk()
 
-        self.root.title('Manual Velocity')
+        self.root.title(buggy_name + ' Manual Velocity: scale = 0.1m/s')
         self.root.geometry('600x100')
         self.root.configure(background = '#342d66')
 
@@ -24,7 +24,7 @@ def __init__(self, init_vel: float, buggy_name: str):
         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 
+        """sets velocity of buggy to the current scale value
             called once every tick
         """
         self.root.update_idletasks()