diff --git a/README.md b/README.md
index b2a2c643..9f0606ff 100755
--- a/README.md
+++ b/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,13 +55,13 @@ 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`.
@@ -69,17 +69,16 @@ A complete re-write of the old RoboBuggy.
## 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
-- 
+
+
- 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`
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
index 2cda83d1..5e10b549 100755
--- a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
+++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,12 +1,12 @@
-
+
-
-
+
+
@@ -30,7 +30,7 @@
+ args="$(arg nand_start_pos) $(arg nand_velocity) NAND"/>
@@ -38,7 +38,7 @@
args="$(arg nand_autonsystem_args)"/>
+ args="$(arg sc_start_pos) $(arg sc_velocity) SC"/>
+
-
@@ -22,7 +22,7 @@
+ args="$(arg start_pos) $(arg velocity) $(arg buggy_name)"/>
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index 388436c2..87a8ecad 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/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-?[\d\.]+), *(?P-?[\d\.]+), *(?P-?[\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()