Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Sim launch position arg #43

Merged
merged 14 commits into from
Feb 26, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 11 additions & 12 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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:
Expand All @@ -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".
Expand All @@ -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`
Expand Down
10 changes: 5 additions & 5 deletions rb_ws/src/buggy/launch/sim_2d_2buggies.launch
Original file line number Diff line number Diff line change
@@ -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" />

Expand All @@ -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"
Expand Down
4 changes: 2 additions & 2 deletions rb_ws/src/buggy/launch/sim_2d_single.launch
Original file line number Diff line number Diff line change
@@ -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" />

Expand All @@ -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)"/>
Expand Down
46 changes: 42 additions & 4 deletions rb_ws/src/buggy/scripts/2d_sim/engine.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#! /usr/bin/env python3

import re
import sys
import threading
import rospy
Expand All @@ -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:
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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

Expand Down Expand Up @@ -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()

Loading