From a5c12013819453629db9f39149ffe03e54699367 Mon Sep 17 00:00:00 2001 From: PatXue <95881915+PatXue@users.noreply.github.com> Date: Mon, 26 Feb 2024 17:07:08 -0500 Subject: [PATCH] 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 --- README.md | 23 +++++----- rb_ws/src/buggy/launch/sim_2d_2buggies.launch | 10 ++-- rb_ws/src/buggy/launch/sim_2d_single.launch | 4 +- rb_ws/src/buggy/scripts/2d_sim/engine.py | 46 +++++++++++++++++-- 4 files changed, 60 insertions(+), 23 deletions(-) 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 -- Screenshot 2023-11-13 at 3 18 30 PM + +Screenshot 2023-11-13 at 3 18 30 PM - 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()