Sim launch position arg (#43)

* Setup system to get starting pose from start dist

Code still not tested, could not figure out how to import world and trajectory into engine.py

* Copy trajectory and dependencies into 2d_sim

Ideally would figure out how to not have to do this and just import them directly from auton directory

* Do world to utm conversion without world.py

Rather than dealing with the two different Pose classes (from pose.py and rospy), just do the conversion directly in engine.oy

* Start auton from start_dist as well

The sim now seems to start the buggy at the correct place, but auton doesn't seem to take it well and causes buggy to steer in very tight circles (buggy appears static on map)

* Fix pylint complaint

* Get imports in engine.py to work

* Fix merge issues

* Add distance args to 2buggy launch file

* Fix pylint complaint

* Add ability to use start_pos as dict key

Uses start_pos arg as dict key if valid key, and as distance otherwise

* start_pos arg(s) can now also take utm coords as input

* Add info about start_pos argument in readme

---------

Co-authored-by: Gabriel Zaragoza <gzaragoz@andrew.cmu.edu>

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 nuc@192.168.1.217`
 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()