Merge branch 'main' into sw-preroll

README.md

@@ -31,7 +31,7 @@ A complete re-write of the old RoboBuggy.
 ### ROS
 - Navigate to `/rb_ws`. This is the catkin workspace where we will be doing all our ROS stuff.
 - To build the ROS workspace and source it, run:
-        
+
         catkin_make
         source /rb_ws/devel/setup.bash  # sets variables so that our package is visible to ROS commands
 - To learn ROS on your own, follow the guide on https://wiki.ros.org/ROS/Tutorials. Start from the first and install Ros using a Virtual Machine.
@@ -55,31 +55,30 @@ A complete re-write of the old RoboBuggy.
 ### ROS
 - Navigate to `/rb_ws`. This is the catkin workspace where we will be doing all our ROS stuff.
 - To build the ROS workspace and source it, run:
-        
+
         catkin_make
         source /rb_ws/devel/setup.bash  # sets variables so that our package is visible to ROS commands
 - To learn ROS on your own, follow the guide on https://wiki.ros.org/ROS/Tutorials. Start from the first and install Ros using a Virtual Machine.
 
 ---
-## Open Docker 
+## Open Docker
 - Use `cd` to change the working directory to be `RoboBuggy2`
 - Then do `./setup_dev.sh` in the main directory (RoboBuggy2) to launch the docker container. Utilize the `--no-gpu`, `--force-gpu`, and `--run-testing` flags as necessary.
 - Then you can go in the docker container using the `docker exec -it robobuggy2-main-1 bash`.
 - When you are done, type Ctrl+C and use `$exit` to exit.
 
 ## 2D Simulation
 - Boot up the docker container
-- Run `roslaunch buggy sim_2d_single.launch` to simulate 1 buggy 
+- Run `roslaunch buggy sim_2d_single.launch` to simulate 1 buggy
 - See `rb_ws/src/buggy/launch/sim_2d_single.launch` to view all available launch options
-
 - Run `roslaunch buggy sim_2d_2buggies.launch` to simulate 2 buggies
-- <img width="612" alt="Screenshot 2023-11-13 at 3 18 30 PM" src="https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/45720415/b204aa05-8792-414e-a868-6fbc0d11ab9d">
+
+<img width="612" alt="Screenshot 2023-11-13 at 3 18 30 PM" src="https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/45720415/b204aa05-8792-414e-a868-6fbc0d11ab9d">
 
 - See `rb_ws/src/buggy/launch/sim_2d_2buggies.launch` to view all available launch options
-- To prevent topic name collision, a topic named `t` associated with buggy named `x` have format `x\t`. The 
-- names are `SC` and `Nand` in the 2 buggy simulator. In the one buggy simulator, the name can be defined as a launch arg.
-- See [**Foxglove Visualization**](#foxglove-visualization) for visualizing the simulation. Beware that since topic names
-- are user-defined, you will need to adjust the topic names in each panel.
+    - The buggy starting positions can be changed using the `sc_start_pos` and `nand_start_pos` arguments (can pass as a key to a dictionary of preset start positions in engine.py, a single float for starting distance along planned trajectory, or 3 comma-separated floats (utm east, utm north, and heading))
+- To prevent topic name collision, a topic named `t` associated with buggy named `x` have format `x/t`. The names are `SC` and `Nand` in the 2 buggy simulator. In the one buggy simulator, the name can be defined as a launch arg.
+- See [**Foxglove Visualization**](#foxglove-visualization) for visualizing the simulation. Beware that since topic names are user-defined, you will need to adjust the topic names in each panel.
 
 ### Simulator notes
 Feedback:
@@ -90,7 +89,7 @@ Commands:
 - Steering angle: `/buggy/steering` in degrees (std_msgs/Float64)
 - Velocity: `/buggy/velocity` in m/s (std_msgs/Float64)
 
-  
+
 ## Foxglove Visualization
 - Foxglove is used to visualize both the simulator and the actual buggy's movements.
 - First, you need to import the layout definition into Foxglove. On the top bar, click Layout, then "Import from file".
@@ -114,7 +113,7 @@ Instructions:
 ### Connecting to and Launching the RoboBuggy
 When launching the buggy:
 - Connect to the Wi-Fi named ShortCircuit.
--	In the command line window: 
+-	In the command line window:
 SSH to the computer on ShortCircuit and go to folder
 `$ ssh [email protected]`
 Then `$ cd RoboBuggy2`

rb_ws/src/buggy/launch/sim_2d_2buggies.launch

@@ -1,12 +1,12 @@
 <launch>
-    <arg name="sc_starting_pose"    default="Hill1_SC" />
+    <arg name="sc_start_pos"       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_start_pos"     default="Hill1_NAND" />
     <arg name="nand_velocity"       default="10.0" />
+
     <arg name="manual_vel"          default="true" />
     <arg name="auto_vel"            default="false" />
 
@@ -30,15 +30,15 @@
 
 
     <node name="nand_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg nand_starting_pose) $(arg nand_velocity) NAND"/>
+        args="$(arg nand_start_pos) $(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"/>
+        args="$(arg sc_start_pos) $(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"

rb_ws/src/buggy/launch/sim_2d_single.launch

@@ -1,6 +1,6 @@
 <launch>
+    <arg name="start_pos"        default="Hill1_SC" />
     <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" />
 
@@ -22,7 +22,7 @@
     </group>
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
+        args="$(arg start_pos) $(arg velocity) $(arg buggy_name)"/>
 
     <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg autonsystem_args)"/>

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

@@ -1,5 +1,6 @@
 #! /usr/bin/env python3
 
+import re
 import sys
 import threading
 import rospy
@@ -9,6 +10,9 @@
 from nav_msgs.msg import Odometry
 import numpy as np
 import utm
+sys.path.append("/rb_ws/src/buggy/scripts/auton")
+from trajectory import Trajectory
+from world import World
 
 
 class Simulator:
@@ -22,7 +26,7 @@ class Simulator:
     START_LONG = -79.9409643423245
     NOISE = True # Noisy outputs for nav/odom?
 
-    def __init__(self, starting_pose, velocity, buggy_name):
+    def __init__(self, start_pos: str, velocity: float, buggy_name: str):
         """
         Args:
             heading (float): degrees start heading of buggy
@@ -69,7 +73,37 @@ 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]
+
+        # Use start_pos as key in starting_poses dictionary
+        if start_pos in self.starting_poses:
+            init_pose = self.starting_poses[start_pos]
+        else:
+            # Use start_pos as float representing distance down track to start from
+            try:
+                start_pos = float(start_pos)
+                trajectory = Trajectory("/rb_ws/src/buggy/paths/buggycourse_safe_1.json")
+
+                init_world_coords = trajectory.get_position_by_distance(start_pos)
+                init_heading = np.rad2deg(trajectory.get_heading_by_distance(start_pos)[0])
+                init_x, init_y = tuple(World.world_to_utm_numpy(init_world_coords)[0])
+
+            # Use start_pos as (e_utm, n_utm, heading) coordinates
+            except ValueError:
+                # Extract the three coordinates from start_pos
+                matches = re.match(
+                    r"^\(?(?P<utm_e>-?[\d\.]+), *(?P<utm_n>-?[\d\.]+), *(?P<heading>-?[\d\.]+)\)?$",
+                    start_pos
+                )
+                if matches == None: raise ValueError("invalid start_pos for " + buggy_name)
+                matches = matches.groupdict()
+
+                init_x = float(matches["utm_e"])
+                init_y = float(matches["utm_n"])
+                init_heading = float(matches["heading"])
+
+            init_pose = init_x, init_y, init_heading
+
+        self.e_utm, self.n_utm, self.heading = init_pose
         self.velocity = velocity # m/s
 
         self.steering_angle = 0  # degrees
@@ -96,6 +130,7 @@ def update_velocity(self, data: Float64):
         """
         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,6 +144,7 @@ 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
 
@@ -287,9 +323,11 @@ def loop(self):
     rospy.init_node("sim_2d_engine")
     print("sim 2d eng args:")
     print(sys.argv)
-    starting_pose = sys.argv[1]
+
+    start_pos = sys.argv[1]
     velocity = float(sys.argv[2])
     buggy_name = sys.argv[3]
-    sim = Simulator(starting_pose, velocity, buggy_name)
+
+    sim = Simulator(start_pos, velocity, buggy_name)
     sim.loop()