diff --git a/.github/pull_request_template b/.github/pull_request_template new file mode 100644 index 00000000..8ead8f01 --- /dev/null +++ b/.github/pull_request_template @@ -0,0 +1,65 @@ + + +## What type of PR is this? (check all applicable) + +- [ ] Refactor +- [ ] Feature +- [ ] Bug Fix +- [ ] Optimization +- [ ] Documentation Update + +## Description +_Describe the CHANGES, the REASONING, and BENEFITS of this PR._ + +## Related Tickets & Documents + + + +- Related Issue # +- Closes # + +## QA Instructions, Screenshots, Recordings + +_Please replace this line with instructions on how to test your changes, a note +on the devices and browsers this has been tested on, as well as any relevant +images for UI changes._ + +## Added/updated tests? +_We encourage you to keep the code coverage percentage at 80% and above._ + +- [ ] Yes +- [ ] No, and this is why: _please replace this line with details on why tests + have not been included_ +- [ ] I need help with writing tests + +## [optional] Are there any post deployment tasks we need to perform? + +## [optional] What gif best describes this PR or how it makes you feel? + +![alt_text](gif_link) diff --git a/README.md b/README.md index 5dbc8832..74e2f3c1 100755 --- a/README.md +++ b/README.md @@ -9,20 +9,24 @@ A complete re-write of the old RoboBuggy. --- -## Installation +## Installation (for Windows) +### Install Softwares: WSL, Ubuntu, Foxglove +- Go to Microsoft Store to install "Ubuntu 20.04.6 LTS". +- Go install Foxglove https://foxglove.dev/. + ### Docker - You will need [Docker](https://docs.docker.com/get-docker/) installed. -- Once installed, the Docker image can be built as follows from the repository's root directory: - - docker compose build --no-cache - docker compose --env-file .env.dev up - -- Then in another terminal window, in order to access the running Docker's CLI, run: +### Set up repo in WSL +- To set up ssh key, follow this link: [Connecting to GitHub with SSH](https://docs.github.com/en/authentication/connecting-to-github-with-ssh). +- Note: Ensure that the SSH keys are generated while in the WSL terminal +- In the website above, see these two pages: [Generating a new SSH key and adding it to the ssh-agent](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) and ["Adding a new SSH key to your GitHub account"](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/adding-a-new-ssh-key-to-your-github-account). - docker exec -it robobuggy2-main-1 bash +### Clone +- In your terminal type: `$ git clone https://github.com/CMU-Robotics-Club/RoboBuggy2.git`. +- The clone link above is find in github: code -> local -> Clone SSH. +- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/8ea809f7-35f9-4517-b98d-42e2e869d233) -- Now you should be presented with a bash CLI as you're used to. ### ROS - Navigate to `/rb_ws`. This is the catkin workspace where we will be doing all our ROS stuff. @@ -30,35 +34,74 @@ A complete re-write of the old RoboBuggy. 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. -### Alternate Shortcut -- In the main directory, just run `./setup_dev.sh` or `./setup_prod.sh` as appropriate. -- Run `exec_docker` to get into Docker environment and setup all the aliases. - - ---- -## 3D Simulation Quickstart - -Watch [this](https://youtu.be/kEL3-sF9TTE) video to get started with visualizing the simulation. - +## Installation (for MacOS - M2) +### Install Softwares: Docker, Foxglove +- Go install Foxglove https://foxglove.dev/. +- You will need [Docker](https://docs.docker.com/get-docker/) installed. -### Controls +### Set up repository +- To set up ssh key, follow this link: [Connecting to GitHub with SSH](https://docs.github.com/en/authentication/connecting-to-github-with-ssh). +- Note: Ensure that the SSH keys are generated while in the WSL terminal +- In the website above, see these two pages: [Generating a new SSH key and adding it to the ssh-agent](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) and ["Adding a new SSH key to your GitHub account"](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/adding-a-new-ssh-key-to-your-github-account). -Edit `rb_ws/src/buggy/scripts/controller.py`. Skeleton code for interacting with existing topics is provided. +### Clone +- In your terminal type: `$ git clone git@github.com:CMU-Robotics-Club/RoboBuggy.git`. +- The clone link above is find in github: code -> local -> Clone SSH. +- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/8ea809f7-35f9-4517-b98d-42e2e869d233) -To launch the simulation against your controls code, run the following command: `roslaunch buggy main.launch simulation:=true`. -If you get an error about not finding package `buggy`, remember to run `source /rb_ws/devel/setup.bash`. +### 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. --- -## 2D Simulation Quickstart -Examples (from the same run): -- Foxglove playback of simulated file example: [link](https://youtu.be/dpa5oH69eJI) -- Matplotlib live simulation example: [link](https://youtu.be/6Xji-FtDQfo) - -Control Example: -- Foxglove output (via sending steering + velocity commands): [link](https://youtu.be/AOsecwWmqyw) +## 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. +- 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 +- 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 + +- 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. + +### Simulator notes +Feedback: +- Longitude + Latitude for Foxglove visualization on map: `/state/pose_navsat` (sensor_msgs/NavSatFix) +- UTM coordinates (assume we're in Zone 17T): `/sim_2d/utm` (geometry_msgs/Pose - position.x = Easting meters , position.y = Northing meters, position.z = heading in degrees from East axis + is CCW) +- INS Simulation: `/nav/odom` (nsg_msgs/Odometry) (**Noise** is implemented to vary ~1cm) +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". +- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/2aa04083-46b3-42a5-bcc1-99cf7ccdb3d2) +- Go to RoboBuggy2 and choose the file [telematics layout](telematics_layout.json) +- To visualize the simulator, launch the simulator and then launch Foxglove and select "Open Connection" on startup. +- Use this address `ws://localhost:8765` for Foxglove Websocket +- Open Foxglove, choose the third option "start link". +- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/66965d34-502b-4130-976e-1419c0ac5f69) + +## X11 Setup Instructions: - Install the appropriate X11 server on your computer for your respective operating systems (Xming for Windows, XQuartz for Mac, etc.). - Mac: In XQuartz settings, ensure that the "Allow connections from network clients" under "Security" is checked. @@ -66,27 +109,27 @@ Instructions: - While in a bash shell with the X11 server running, run `xhost +local:docker`. - Boot up the docker container using the "Alternate Shortcut" above. - Run `xeyes` while INSIDE the Docker container to test X11 forwarding. If this works, we're good. -- Run `roslaunch buggy sim_2d.launch controller:=CONTROLLER_TYPE` for the simulator. - -### Using the Simulator -Feedback: -- Longitude + Latitude for Foxglove visualization on map: `/state/pose_navsat` (sensor_msgs/NavSatFix) -- UTM coordinates (assume we're in Zone 17T): `/sim_2d/utm` (geometry_msgs/Pose - position.x = Easting meters , position.y = Northing meters, position.z = heading in degrees from East axis + is CCW) -- INS Simulation: `/nav/odom` (nsg_msgs/Odometry) (**Noise** is implemented to vary ~1cm) - -Commands: -- Steering angle: `/buggy/steering` in degrees (std_msgs/Float64) -- Velocity: `/buggy/velocity` in m/s (std_msgs/Float64) --- - -## Development - -TODO: - - pusher.py: apply simulated pushing force if buggy is in certain areas - - steering_mux.py: Decide where to listen for steering inputs given a state - - dead-mans (lock right(?)) - - Teleop (Input from foxglove teleop panel) - - Controls (auton) - - instrument_*.py - - INS, +### Connecting to and Launching the RoboBuggy +When launching the buggy: +- Connect to the Wi-Fi named ShortCircuit. +- In the command line window: +SSH to the computer on ShortCircuit and go to folder +`$ ssh nuc@192.168.1.217` +Then `$ cd RoboBuggy2` +- Setup the docker +`$ ./setup_prod.sh` +- Go to docker container +`$ docker_exec` +- Open foxglove and do local connection to โ€œws://192.168.1.217/8765โ€ +- Roslauch in docker container by `$ roslaunch buggy main.launch` +(wait until no longer prints โ€œwaiting for covariance to be betterโ€) + +When shutting down the buggy: +- Stop roslauch +`$ ^C (Ctrl+C)` +- Leave the docker container +`$ exit` +- Shutdown the ShortCircuit computer +`$ sudo shutdown now` diff --git a/python-requirements.txt b/python-requirements.txt index ab966e4c..cbc5d7de 100755 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -1,10 +1,10 @@ -trimesh -pyembree -numpy>=1.20.0 -pandas -pymap3d -matplotlib -utm -scipy -osqp -numba +matplotlib==3.1.2 +numba==0.58.0 +numpy<1.21.0 +osqp==0.6.3 +pandas==2.0.3 +pyembree==0.2.11 +pymap3d==3.0.1 +scipy==1.10.1 +trimesh==3.23.5 +utm==0.7.0 diff --git a/rb_ws/src/buggy/assets/cost_grid.png b/rb_ws/src/buggy/assets/cost_grid.png new file mode 100644 index 00000000..b54920cb Binary files /dev/null and b/rb_ws/src/buggy/assets/cost_grid.png differ diff --git a/rb_ws/src/buggy/assets/landmarks.json b/rb_ws/src/buggy/assets/landmarks.json new file mode 100644 index 00000000..a2189d2b --- /dev/null +++ b/rb_ws/src/buggy/assets/landmarks.json @@ -0,0 +1,314 @@ +[ + { + "pixel": [ + 940, + 180 + ], + "time": "213721.00", + "quality": "5", + "num_sats": "12", + "key": "walkoflife-213721.00", + "lat": 40.444168, + "lon": -79.94299933333333, + "active": false + }, + { + "pixel": [ + 896, + 257 + ], + "time": "213933.00", + "quality": "5", + "num_sats": "12", + "key": "cfacut-213933.00", + "lat": 40.44388133333334, + "lon": -79.94313766666667, + "active": false + }, + { + "pixel": [ + 850, + 436 + ], + "time": "214347.00", + "quality": "4", + "num_sats": "12", + "key": "cfapausch-214347.00", + "lat": 40.44316983333333, + "lon": -79.94344550000001, + "active": false + }, + { + "pixel": [ + 841, + 492 + ], + "time": "214535.00", + "quality": "5", + "num_sats": "12", + "key": "cutfence-214535.00", + "lat": 40.44286, + "lon": -79.94349766666666, + "active": false + }, + { + "pixel": [ + 855, + 547 + ], + "time": "214704.00", + "quality": "5", + "num_sats": "12", + "key": "midcut-214704.00", + "lat": 40.44258766666667, + "lon": -79.94339450000001, + "active": false + }, + { + "pixel": [ + 825, + 620 + ], + "time": "214813.00", + "quality": "5", + "num_sats": "12", + "key": "fence-214813.00", + "lat": 40.442260499999996, + "lon": -79.94358066666668, + "active": false + }, + { + "pixel": [ + 764, + 655 + ], + "time": "214936.00", + "quality": "5", + "num_sats": "12", + "key": "mallfence-214936.00", + "lat": 40.44212, + "lon": -79.943908, + "active": false + }, + { + "pixel": [ + 693, + 672 + ], + "time": "215056.00", + "quality": "5", + "num_sats": "12", + "key": "dohertyleft-215056.00", + "lat": 40.442053, + "lon": -79.94433016666667, + "active": false + }, + { + "pixel": [ + 579, + 652 + ], + "time": "215229.00", + "quality": "5", + "num_sats": "12", + "key": "mall-215229.00", + "lat": 40.442141666666664, + "lon": -79.94497066666666, + "active": false + }, + { + "pixel": [ + 407, + 608 + ], + "time": "215428.00", + "quality": "2", + "num_sats": "12", + "key": "wean-215428.00", + "lat": 40.4423355, + "lon": -79.94597416666667, + "active": false + }, + { + "pixel": [ + 393, + 663 + ], + "time": "215546.00", + "quality": "4", + "num_sats": "12", + "key": "porter-215546.00", + "lat": 40.44209816666667, + "lon": -79.94608316666667, + "active": false + }, + { + "pixel": [ + 642, + 768 + ], + "time": "215747.00", + "quality": "4", + "num_sats": "12", + "key": "baker-215747.00", + "lat": 40.441795166666665, + "lon": -79.9446295, + "active": false + }, + { + "pixel": [ + 792, + 825 + ], + "time": "215942.00", + "quality": "5", + "num_sats": "12", + "key": "hunt-215942.00", + "lat": 40.4413605, + "lon": -79.94378483333334, + "active": false + }, + { + "pixel": [ + 987, + 856 + ], + "time": "220153.00", + "quality": "5", + "num_sats": "12", + "key": "numbergarden-220153.00", + "lat": 40.4412145, + "lon": -79.94264816666667, + "active": false + }, + { + "pixel": [ + 1064, + 718 + ], + "time": "220431.00", + "quality": "4", + "num_sats": "12", + "key": "parkinglot-220431.00", + "lat": 40.44181266666667, + "lon": -79.94217383333333, + "active": false + }, + { + "pixel": [ + 1115, + 725 + ], + "time": "220555.00", + "quality": "4", + "num_sats": "12", + "key": "posner-220555.00", + "lat": 40.441783666666666, + "lon": -79.941875, + "active": false + }, + { + "pixel": [ + 1399, + 740 + ], + "time": "220915.00", + "quality": "5", + "num_sats": "12", + "key": "donner-220915.00", + "lat": 40.44170666666666, + "lon": -79.94021533333333, + "active": false + }, + { + "pixel": [ + 1503, + 736 + ], + "time": "221056.00", + "quality": "4", + "num_sats": "12", + "key": "whesco-221056.00", + "lat": 40.441725166666664, + "lon": -79.939629, + "active": false + }, + { + "pixel": [ + 1555, + 704 + ], + "time": "221202.00", + "quality": "4", + "num_sats": "12", + "key": "boss-221202.00", + "lat": 40.4418575, + "lon": -79.93930783333333, + "active": false + }, + { + "pixel": [ + 1498, + 613 + ], + "time": "221458.00", + "quality": "4", + "num_sats": "12", + "key": "compass-221458.00", + "lat": 40.4422465, + "lon": -79.939632, + "active": false + }, + { + "pixel": [ + 1454, + 588 + ], + "time": "221745.00", + "quality": "5", + "num_sats": "12", + "key": "resnik-221745.00", + "lat": 40.442363166666674, + "lon": -79.93987533333333, + "active": false + }, + { + "pixel": [ + 1524, + 530 + ], + "time": "221921.00", + "quality": "4", + "num_sats": "12", + "key": "track-221921.00", + "lat": 40.4426285, + "lon": -79.93948016666667, + "active": false + }, + { + "pixel": [ + 1349, + 575 + ], + "time": "222114.00", + "quality": "4", + "num_sats": "12", + "key": "bballcourt-222114.00", + "lat": 40.44244466666667, + "lon": -79.94051433333334, + "active": false + }, + { + "pixel": [ + 1235, + 528 + ], + "time": "222257.00", + "quality": "2", + "num_sats": "12", + "key": "truck-222257.00", + "lat": 40.442629333333336, + "lon": -79.94115933333333, + "active": false + } +] \ No newline at end of file diff --git a/rb_ws/src/buggy/assets/sat_img.png b/rb_ws/src/buggy/assets/sat_img.png new file mode 100644 index 00000000..1910c085 Binary files /dev/null and b/rb_ws/src/buggy/assets/sat_img.png differ diff --git a/rb_ws/src/buggy/assets/sat_img_resized.png b/rb_ws/src/buggy/assets/sat_img_resized.png new file mode 100644 index 00000000..b54920cb Binary files /dev/null and b/rb_ws/src/buggy/assets/sat_img_resized.png differ diff --git a/rb_ws/src/buggy/assets/satellite_unflip_crop_blacked.png b/rb_ws/src/buggy/assets/satellite_unflip_crop_blacked.png new file mode 100644 index 00000000..b3ded93b Binary files /dev/null and b/rb_ws/src/buggy/assets/satellite_unflip_crop_blacked.png differ diff --git a/rb_ws/src/buggy/assets/satellite_unflip_crop_raw.png b/rb_ws/src/buggy/assets/satellite_unflip_crop_raw.png new file mode 100644 index 00000000..7273d98b Binary files /dev/null and b/rb_ws/src/buggy/assets/satellite_unflip_crop_raw.png differ diff --git a/rb_ws/src/buggy/launch/main.launch b/rb_ws/src/buggy/launch/main.launch index a0a2c812..14f4cc7f 100644 --- a/rb_ws/src/buggy/launch/main.launch +++ b/rb_ws/src/buggy/launch/main.launch @@ -3,18 +3,10 @@ + - @@ -27,5 +19,6 @@ - + + \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch deleted file mode 100644 index abaff00c..00000000 --- a/rb_ws/src/buggy/launch/sim_2d.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch new file mode 100644 index 00000000..61ad3111 --- /dev/null +++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch new file mode 100644 index 00000000..3554bfcc --- /dev/null +++ b/rb_ws/src/buggy/launch/sim_2d_single.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py index a51dacd0..9598c485 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py +++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py @@ -5,9 +5,9 @@ import numpy as np class Controller: - def __init__(self): - self.steering_publisher = rospy.Publisher("sim_2d/steering", Float64, queue_size=10) - self.velocity_publisher = rospy.Publisher("buggy/velocity", Float64, queue_size=10) + def __init__(self, buggy_name): + self.steering_publisher = rospy.Publisher(buggy_name + "/input/steering", Float64, queue_size=10) + self.velocity_publisher = rospy.Publisher(buggy_name + "/velocity", Float64, queue_size=10) self.steering_angle = 0 self.velocity = 0 @@ -45,7 +45,8 @@ def set_velocity(self, vel: float): if __name__ == "__main__": rospy.init_node("sim_2d_controller") - controller = Controller() + buggy_name = sys.argv[1] + controller = Controller(buggy_name) rate = rospy.Rate(5) i = 0 while not rospy.is_shutdown(): diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index 5d67ce14..8bb34526 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -8,6 +8,7 @@ import numpy as np import utm import time +import sys class Simulator: @@ -21,37 +22,39 @@ class Simulator: START_LONG = -79.9409643423245 NOISE = True # Noisy outputs for nav/odom? - def __init__(self, heading: float): + def __init__(self, starting_pose, velocity, buggy_name): """ Args: heading (float): degrees start heading of buggy """ # for X11 matplotlib (direction included) - self.plot_publisher = rospy.Publisher("sim_2d/utm", Pose, queue_size=1) + self.plot_publisher = rospy.Publisher(buggy_name + "/sim_2d/utm", Pose, queue_size=1) # simulate the INS's outputs (noise included) - self.pose_publisher = rospy.Publisher("nav/odom", Odometry, queue_size=1) + self.pose_publisher = rospy.Publisher(buggy_name + "/nav/odom", Odometry, queue_size=1) self.steering_subscriber = rospy.Subscriber( - "buggy/input/steering", Float64, self.update_steering_angle + buggy_name + "/input/steering", Float64, self.update_steering_angle ) self.velocity_subscriber = rospy.Subscriber( - "buggy/velocity", Float64, self.update_velocity + buggy_name + "/velocity", Float64, self.update_velocity ) # to plot on Foxglove (no noise) self.navsatfix_publisher = rospy.Publisher( - "state/pose_navsat", NavSatFix, queue_size=1 + buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1 ) # to plot on Foxglove (with noise) self.navsatfix_noisy_publisher = rospy.Publisher( - "state/pose_navsat_noisy", NavSatFix, queue_size=1 + buggy_name + "/state/pose_navsat_noisy", NavSatFix, queue_size=1 ) - # Start position for Start of Course - self.e_utm = Simulator.UTM_EAST_ZERO + 60 - self.n_utm = Simulator.UTM_NORTH_ZERO + 150 + # (UTM east, UTM north, HEADING(degs)) + self.starting_poses = { + "Hill1_SC": (Simulator.UTM_EAST_ZERO + 60, Simulator.UTM_NORTH_ZERO + 150, -110), + "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 55, Simulator.UTM_NORTH_ZERO + 165, -125), + } # Start position for End of Hill 2 # self.e_utm = Simulator.UTM_EAST_ZERO - 3 @@ -66,11 +69,10 @@ def __init__(self, heading: float): # self.e_utm = utm_coords[0] # self.n_utm = utm_coords[1] - self.heading = heading # degrees - self.velocity = 15 # m/s + self.e_utm, self.n_utm, self.heading = self.starting_poses[starting_pose] + self.velocity = velocity # m/s self.steering_angle = 0 # degrees - self.rate = 100 # Hz self.pub_skip = 10 # publish every pub_skip ticks @@ -161,7 +163,6 @@ def step(self): def publish(self): """Publishes the pose the arrow in visualizer should be at""" p = Pose() - time_stamp = rospy.Time.now() with self.lock: @@ -292,5 +293,10 @@ def loop(self): if __name__ == "__main__": rospy.init_node("sim_2d_engine") - sim = Simulator(-110) + print("sim 2d eng args:") + print(sys.argv) + starting_pose = sys.argv[1] + velocity = float(sys.argv[2]) + buggy_name = sys.argv[3] + sim = Simulator(starting_pose, velocity, buggy_name) sim.loop() diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py new file mode 100755 index 00000000..a6bebe18 --- /dev/null +++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py @@ -0,0 +1,74 @@ +#! /usr/bin/env python3 +import rospy +import sys +from controller_2d import Controller +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Point +import threading +import math + +class VelocityUpdater: + RATE = 100 + # Bubbles for updating acceleration based on position + # represented as 4-tuples: (x-pos, y-pos, radius, acceleration) + CHECKPOINTS: 'list[tuple[float,float,float,float]]' = [ + (589701, 4477160, 20, 0.5) + ] + + def __init__(self, init_vel: float, buggy_name: str): + self.pose_subscriber = rospy.Subscriber( + buggy_name + "/sim_2d/utm", Pose, self.update_position + ) + + self.buggy_vel = init_vel + self.accel = 0.0 + + self.position = Point() + self.position.x = 0 + self.position.y = 0 + self.position.z = 0 + + self.controller = Controller(buggy_name) + + self.lock = threading.Lock() + + def update_position(self, new_pose: Pose): + '''Callback function to update internal position variable when new + buggy position is published + + Args: + new_pose (Pose): Pose object from topic + ''' + with self.lock: + self.position = new_pose.position + + def calculate_accel(self): + '''Check if the position of the buggy is in any of the checkpoints set + in self.CHECKPOINTS, and update acceleration of buggy accordingly + ''' + for (x,y,r,a) in self.CHECKPOINTS: + dist = math.sqrt((x - self.position.x)**2 + (y - self.position.y)**2) + if dist < r: + self.accel = a + break + + def step(self): + '''Update acceleration and velocity of buggy for one timestep''' + self.calculate_accel() + + new_velocity = self.buggy_vel + self.accel / self.RATE + self.buggy_vel = new_velocity + self.controller.set_velocity(new_velocity) + + +if __name__ == "__main__": + rospy.init_node("vel_updater") + + init_vel = float(sys.argv[1]) + buggy_name = sys.argv[2] + vel = VelocityUpdater(init_vel, buggy_name) + rate = rospy.Rate(vel.RATE) + + while not rospy.is_shutdown(): + vel.step() + rate.sleep() diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 0338ed09..ba43285c 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -40,36 +40,36 @@ class AutonSystem: ticks = 0 - def __init__(self, trajectory, controller, brake_controller) -> None: + def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim) -> None: self.trajectory = trajectory self.controller = controller self.brake_controller = brake_controller self.lock = Lock() self.ticks = 0 - - self.msg = None + + if (is_sim): + rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg) + - rospy.Subscriber("nav/odom", Odometry, self.update_msg) self.covariance_warning_publisher = rospy.Publisher( - "buggy/debug/is_high_covariance", Bool, queue_size=1 + buggy_name + "/debug/is_high_covariance", Bool, queue_size=1 ) self.steer_publisher = rospy.Publisher( - "buggy/input/steering", Float64, queue_size=1 + buggy_name + "/input/steering", Float64, queue_size=1 ) self.brake_publisher = rospy.Publisher( - "buggy/input/brake", Float64, queue_size=1 + buggy_name + "/input/brake", Float64, queue_size=1 ) self.brake_debug_publisher = rospy.Publisher( - "auton/debug/brake", Float64, queue_size=1 + buggy_name + "/auton/debug/brake", Float64, queue_size=1 ) self.heading_publisher = rospy.Publisher( - "auton/debug/heading", Float32, queue_size=1 + buggy_name + "/auton/debug/heading", Float32, queue_size=1 ) - self.distance_publisher = rospy.Publisher( - "auton/debug/distance", Float64, queue_size=1 + buggy_name + "/auton/debug/distance", Float64, queue_size=1 ) self.auton_rate = 100 @@ -152,31 +152,36 @@ def tick(self): arg_ctrl = sys.argv[1] arg_start_dist = sys.argv[2] + arg_path = sys.argv[3] start_dist = float(arg_start_dist) + buggy_name = sys.argv[4] + is_sim = sys.argv[5] == "True" print("\n\nStarting Controller: " + str(arg_ctrl) + "\n\n") + print("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(arg_path) + "\n\n") print("\n\nStarting at distance: " + str(arg_start_dist) + "\n\n") - trajectory = Trajectory("/rb_ws/src/buggy/paths/frew_parkinglot_1.json") + trajectory = Trajectory("/rb_ws/src/buggy/paths/" + arg_path) # calculate starting index start_index = trajectory.get_index_from_distance(start_dist) - # Add Controllers Here ctrller = None if (arg_ctrl == "stanley"): - ctrller = StanleyController(start_index) + ctrller = StanleyController(buggy_name, start_index) elif (arg_ctrl == "pure_pursuit"): - ctrller = PurePursuitController(start_index) + ctrller = PurePursuitController(buggy_name, start_index) elif (arg_ctrl == "mpc"): - ctrller = ModelPredictiveController(start_index) + ctrller = ModelPredictiveController(buggy_name, start_index) if (ctrller == None): raise Exception("Invalid Controller Argument") auton_system = AutonSystem( trajectory, ctrller, - BrakeController() + BrakeController(), + buggy_name, + is_sim ) while not rospy.is_shutdown(): rospy.spin() diff --git a/rb_ws/src/buggy/scripts/auton/controller.py b/rb_ws/src/buggy/scripts/auton/controller.py index 8fc865ca..5cd3aaf5 100755 --- a/rb_ws/src/buggy/scripts/auton/controller.py +++ b/rb_ws/src/buggy/scripts/auton/controller.py @@ -19,30 +19,29 @@ class Controller(ABC): Example schemes include Pure Pursuit, Stanley, and LQR. """ + # TODO: add case for buggy intrinsics NAND_WHEELBASE = 1.3 - SS_WHEELBASE = 1.3 - - WHEELBASE = NAND_WHEELBASE - + SC_WHEELBASE = 1.104 + WHEELBASE = SC_WHEELBASE current_traj_index = 0 - def __init__(self, start_index) -> None: + def __init__(self, start_index, buggy_name) -> None: + self.buggy_name = buggy_name self.trajectory_forward_1 = rospy.Publisher( - "auton/debug/forward1_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/forward1_navsat", NavSatFix, queue_size=1 ) self.trajectory_forward_2 = rospy.Publisher( - "auton/debug/forward2_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/forward2_navsat", NavSatFix, queue_size=1 ) self.trajectory_forward_3 = rospy.Publisher( - "auton/debug/forward3_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/forward3_navsat", NavSatFix, queue_size=1 ) self.trajectory_backward_1 = rospy.Publisher( - "auton/debug/backward1_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/backward1_navsat", NavSatFix, queue_size=1 ) # Make lists of publishers for easy iteration self.forward_publishers = [self.trajectory_forward_1, self.trajectory_forward_2, self.trajectory_forward_3] self.backward_publishers = [self.trajectory_backward_1] - self.current_traj_index = start_index @abstractmethod diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py index fa1ea314..e2af6ea5 100644 --- a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py +++ b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py @@ -87,9 +87,9 @@ class ModelPredictiveController(Controller): state_cost_diag = np.diag(state_cost) control_cost_diag = np.diag(control_cost) - def __init__(self, start_index=0, ref_trajectory=None, ROS=False) -> None: + def __init__(self, buggy_name, start_index=0, ref_trajectory=None, ROS=False) -> None: # instantiate parent - super(ModelPredictiveController, self).__init__(start_index) + super(ModelPredictiveController, self).__init__(start_index, buggy_name) # Internal variables self.current_traj_index = 0 # Where in the trajectory we are currently @@ -99,10 +99,10 @@ def __init__(self, start_index=0, ref_trajectory=None, ROS=False) -> None: self.solver: osqp.OSQP = None self.debug_reference_pos_publisher = rospy.Publisher( - "auton/debug/reference_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) self.debug_error_publisher = rospy.Publisher( - "auton/debug/error", ROSPose, queue_size=1 + buggy_name + "/auton/debug/error", ROSPose, queue_size=1 ) self.solver = osqp.OSQP() diff --git a/rb_ws/src/buggy/scripts/auton/occupancy_grid/grid_manager.py b/rb_ws/src/buggy/scripts/auton/occupancy_grid/grid_manager.py new file mode 100644 index 00000000..212f5f0d --- /dev/null +++ b/rb_ws/src/buggy/scripts/auton/occupancy_grid/grid_manager.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 + +import numpy as np +import cv2 +import sys +from matplotlib import pyplot as plt +import utm +sys.path.append('rb_ws/src/buggy/scripts/auton') +# from pose import Pose +import json + +""" +#################################### +=============== USAGE ============== +#################################### +Initialize: +- Create occupancy grid object + +Normal Op: +- Put NAND in the cost map via set_cost(utm_coords, cost) +- Get the normalized cost via get_utm_cost(utm_coords) by passing in the coordinates of the path +- Call reset_grid() to remove NAND from the grid +- LOOP! +""" + +# Do we want to initialize the grid as zero cost? Or do we want to load the pre-fabricated cost map in? +INIT_WITH_EMPTY_GRID = True +class OccupancyGrid: + def __init__(self): + + # Grid 0, 0 is NW + # 1 pixel = 0.5m + # X = East + # Y = North + + # the original grid, should never change + self.sat_img = np.array(cv2.cvtColor(cv2.imread("rb_ws/src/buggy/assets/sat_img_resized.png"), cv2.COLOR_BGR2GRAY), np.uint8) + + # For original sat_img.png + # 145.77 pixels = 88.62m + # 0.5m resolution desired + # Need to scale image by (2/(145.77/88.62)) = 1.215888 + + if INIT_WITH_EMPTY_GRID: + self.grid_og = np.zeros(np.shape(self.sat_img)) + self.grid = np.zeros(np.shape(self.sat_img)) + else: + self.grid_og = np.array(cv2.cvtColor(cv2.imread("rb_ws/src/buggy/assets/sat_img_resized.png"), cv2.COLOR_BGR2GRAY), np.uint8) + # this grid will vary from call to call since NAND will be plotted in here and removed by reverting it back to grid_og + self.grid = np.array(cv2.cvtColor(cv2.imread("rb_ws/src/buggy/assets/cost_grid.png"), cv2.COLOR_BGR2GRAY), np.uint8) + + + + correspondence_f = open("rb_ws/src/buggy/assets/landmarks.json") + self.correspondence = json.load(correspondence_f) + correspondence_f.close() + + pts_src = [] + pts_dst = [] + + for i in range(0, len(self.correspondence)): + ref_lat = self.correspondence[i]["lat"] + ref_lon = self.correspondence[i]["lon"] + ref_utm = utm.from_latlon(ref_lat, ref_lon) + ref_utm = [ref_utm[0], ref_utm[1], 1] + pts_src.append(ref_utm) + + sat_img_pixel = self.correspondence[i]["pixel"] + sat_img_pixel.append(1) # homogenous coordinates + pts_dst.append(sat_img_pixel) + + self.pts_src = np.array(pts_src) + self.pts_dst = np.array(pts_dst) + + + # REF_LOC_UTM_1 = utm.from_latlon(40.438834, -79.946334) + # REF_LOC_UTM_1 = [REF_LOC_UTM_1[0], REF_LOC_UTM_1[1], 1] + # REF_LOC_UTM_2 = utm.from_latlon(40.440482, -79.942299) + # REF_LOC_UTM_2 = [REF_LOC_UTM_2[0], REF_LOC_UTM_2[1], 1] + # REF_LOC_UTM_3 = utm.from_latlon(40.443738, -79.941774) + # REF_LOC_UTM_3 = [REF_LOC_UTM_3[0], REF_LOC_UTM_3[1], 1] + # REF_LOC_UTM_4 = utm.from_latlon(40.443451, -79.945972) + # REF_LOC_UTM_4 = [REF_LOC_UTM_4[0], REF_LOC_UTM_4[1], 1] + + # SAT_IMG_LOC_1 = [354, 1397, 1] + # SAT_IMG_LOC_2 = [1043, 1048, 1] + # SAT_IMG_LOC_3 = [1128, 290, 1] + # SAT_IMG_LOC_4 = [406, 366, 1] + + # pts_src = np.array([REF_LOC_UTM_1, REF_LOC_UTM_2, REF_LOC_UTM_3, REF_LOC_UTM_4]) + # pts_dst = np.array([SAT_IMG_LOC_1, SAT_IMG_LOC_2, SAT_IMG_LOC_3, SAT_IMG_LOC_4]) + + self.homography, status = cv2.findHomography(self.pts_src, self.pts_dst, 0) + + if not np.allclose(status, 1, atol=1e-5): + raise Exception("Cannot compute homography") + + def get_pixel_from_utm(self, utm_coord: np.array): + """Calculate the pixel coordinates from the utm coordinates + + Args: + utm_coord (np.array): utm coordinates + [[0,0], + [100, 100], + [125, 400]] as an example + + Returns: + pixel_coordinates: coordinates in pixels (x, y) + """ + ones = np.ones(utm_coord.shape[0]) + utm_coord_homogenous = np.array([utm_coord[:, 0], utm_coord[:, 1], ones]) + loc = self.homography @ utm_coord_homogenous + return np.array([loc[0]/loc[2], loc[1]/loc[2]]).T + + def get_pixel_from_latlon(self, latlon_coord: np.array): + """Calculate pixel coordintes from latlong coordinates + + Args: + latlon_coord (np.array): latlong coordinates + [[0,0], + [100, 100], + [125, 400]] as an example + + Returns: + _type_: _description_ + """ + utm_coord = utm.from_latlon(latlon_coord[:, 0], latlon_coord[:, 1]) + ones = np.ones(latlon_coord.shape[0]) + utm_coord_homogenous = np.array([utm_coord[0], utm_coord[1], ones]) + loc = self.homography @ utm_coord_homogenous + return np.array([loc[0]/loc[2], loc[1]/loc[2]]).T + + def plot_points(self, utm_coords: list): + """Mark the points on the grid completely BLACK + + Args: + utm_coords (list): list of UTM coordinates to mark + [[0,0], + [100, 100], + [125, 400]] as an example + """ + utm_coords = np.array(utm_coords) + utm_coords = utm_coords[:, 0:2] + pixel_coords = self.get_pixel_from_utm(utm_coords).astype(int) + self.grid[pixel_coords[:, 0], pixel_coords[:, 1]] = 255 + + def set_cost(self, utm_coords: list, cost: list): + """Set the grid's cost + + Args: + utm_coords (list): list of pixel coordinates to mark (x, y) + [[0,0], + [100, 100], + [125, 400]] as an example + cost (list): list of cost corresponding to coords + """ + utm_coords = np.array(utm_coords) + utm_coords = utm_coords[:, 0:2] + pxl_coords = self.get_pixel_from_utm(utm_coords).astype(int) + print(pxl_coords) + self.grid[pxl_coords[:, 0], pxl_coords[:, 1]] = cost + + def set_cost_persistent(self, utm_coords: list, cost: list): + """Set the grid's cost but further calls to reset_grid() will not remove these points + + Args: + utm_coords (list): list of pixel coordinates to mark (x, y) + [[0,0], + [100, 100], + [125, 400]] as an example + cost (list): list of cost corresponding to coords + """ + utm_coords = np.array(utm_coords) + utm_coords = utm_coords[:, 0:2] + pxl_coords = self.get_pixel_from_utm(utm_coords).astype(int) + self.grid[pxl_coords[:, 0], pxl_coords[:, 1]] = cost + self.grid_og[pxl_coords[:, 0], pxl_coords[:, 1]] = cost + + def reset_grid(self): + """Reset the grid back to the original grid (i.e. we remove NAND from the grid) + """ + self.grid = np.copy(self.grid_og) + + def get_utm_cost(self, utm_coords: list): + """Get the cost of the trajectory passed in as utm_coordinates + + Args: + utm_coords (list): coordinates in utm + [[0,0], + [100, 100], + [125, 400]] as an example + + Returns: + normalized cost: sum of all the pixels / number of pixels crossed + """ + coords = np.array(utm_coords) + utm_coords = coords[:, 0:2] + pxl_coords = self.get_pixel_from_utm(utm_coords).astype(int) + filtered_pxl_coords = np.unique(pxl_coords[:, 0:2], axis=0) + num_pixels_passed_thru = len(filtered_pxl_coords) + total = np.sum(self.grid[filtered_pxl_coords[:, 0], filtered_pxl_coords[:, 1]]) + return total/num_pixels_passed_thru + + def get_pxl_cost(self, pxl_coords: list): + """Get the cost of the trajectory passed in as pxl_coordinates + + Args: + pxl_coords (list): coordinates in utm + [[0,0], + [100, 100], + [125, 400]] as an example + + Returns: + normalized cost: sum of all the pixels / number of pixels crossed + """ + pxl_coords = np.array(pxl_coords) + filtered_pxl_coords = np.unique(pxl_coords[:, 0:2], axis=0) + num_pixels_passed_thru = len(filtered_pxl_coords) + total = np.sum(self.grid[filtered_pxl_coords[:, 0], filtered_pxl_coords[:, 1]]) + return total/num_pixels_passed_thru + + +if __name__ == "__main__": + grid = OccupancyGrid() + calculated_pxl = grid.get_pixel_from_utm(grid.pts_src) + error = calculated_pxl - grid.pts_dst[:, 0:2] + # print(f"ERROR: \n {error}") + + # Ensure the duplicate coordinates are removed + test_points_utm = list(grid.pts_src[0:6, 0:2]) + test_points_utm.append([589591.7, 4477381.5]) # append a duplicate point + test_points_utm = np.array(test_points_utm) + print(test_points_utm) + grid.set_cost(test_points_utm, [100, 100, 100, 100, 100, 100, 100]) + cost = grid.get_utm_cost(test_points_utm) + print(cost) + # cv2.imshow("IMG", grid.grid) + # cv2.waitKey(0) + + # print(grid.grid) + # plt.imshow(grid.grid) + # plt.show() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/occupancy_grid/test.py b/rb_ws/src/buggy/scripts/auton/occupancy_grid/test.py new file mode 100644 index 00000000..8037c822 --- /dev/null +++ b/rb_ws/src/buggy/scripts/auton/occupancy_grid/test.py @@ -0,0 +1,40 @@ +import cv2 +import numpy as np +import sys +sys.path.append('rb_ws/src/buggy/scripts/auton') +from grid_manager import OccupancyGrid +from path_projection import Projector +from pose import Pose +import utm + +if __name__ == '__main__': + # src = np.array([[-47.59, -57.43, 139.59, 149.43, 46.], + # [141.43, 31.03, -1.43, 108.97, 70.], + # [579.23, 412.75, 422.55, 589.03, 500.89]]) + + # dst = np.array([[-100, -100, 100, 100, 0], + # [-100, 100, 100, -100, 0], + # [1, 1, 1, 1, 1]]) # converting to homogeneous coordinates + + # print(dst.T) + + # H, _ = cv2.findHomography(src.T, dst.T) + # dst_est = H @ src + # print(np.allclose(dst_est / (dst_est[2, :]), dst / dst[2, :], atol=1e-2)) # dividing by last component to fix the scaling and adjusting the tolerance + + # test_point = np.array([[-47.59], [141.43], [579.23]]) + # res_test = H @ test_point + # res_test = res_test/res_test[2] + # print(res_test) + WHEELBASE = 1.3 + p = Projector(WHEELBASE) + latlon_start = [40.441798, -79.943976] # on the mall + utm_start = utm.from_latlon(latlon_start[0], latlon_start[1]) + start_pose = Pose(utm_start[0], utm_start[1], 0) + path = p.project(start_pose, 1, 5, 15, 10) + grid = OccupancyGrid() + grid.plot_points(path) + grid.get_cost(path) + + cv2.imshow("IMG", grid.sat_img) + cv2.waitKey(0) \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/path_projection.py b/rb_ws/src/buggy/scripts/auton/path_projection.py new file mode 100644 index 00000000..3530b248 --- /dev/null +++ b/rb_ws/src/buggy/scripts/auton/path_projection.py @@ -0,0 +1,80 @@ +import numpy as np +from pose import Pose + +class Projector: + """ + Class for buggy motion projection + """ + def __init__(self, wheelbase: float): + self.wheelbase = wheelbase + + + def project(self, pose: Pose, command: float, v: float, time: float, resolution: int) -> list: + """ + Project buggy motion analytically. Assumes constant velocity and turning angle for the duration. + + Args: + pose (Pose): Pose containing utm_e, utm_n, and heading, in UTM coords and degrees + command (float): Turning angle, in degrees + v (float): Buggy velocity, in m/s + time (float): Time to look ahead, in s + resolution (int): Number of points to output per second + + Returns: + list: List containing 3-tuples of utm_e, utm_n, heading positions along the projected arc. + """ + dtheta = v * np.tan(np.deg2rad(command)) / self.wheelbase + ts = 1/resolution + t = 0 + output = [] + + for i in range(int(resolution * time)): + t += ts + theta = t * dtheta + np.deg2rad(pose.theta) + if dtheta != 0: + x = pose.x + (v / dtheta) * np.sin(theta) + y = pose.y + (v / dtheta) * (1-np.cos(theta)) + else: + x = pose.x + t * v * np.cos(theta) + y = pose.y + t * v * np.sin(theta) + + output.append((x, y, np.rad2deg(theta))) + + return output + + def project_discrete(self, pose: Pose, command: float, v: float, time: float, resolution: int, sim_rate: int) -> list: + """ + Project buggy motion discretely, performing kinematics at each sim_ts. Assumes constant velocity and turning angle for the duration. + + Args: + pose (Pose): Pose containing utm_e, utm_n, and heading, in UTM coords and degrees + command (float): Turning angle, in degrees + v (float): Buggy velocity, in m/s + time (float): Time to look ahead, in s + resolution (int): Number of points to output per second + sim_rate (int): Number of simulation timesteps per second + + Returns: + list: List containing 3-tuples of utm_e, utm_n, heading positions along the projected arc. + """ + ts = 1/resolution + sim_ts = 1/sim_rate + t = 0 + output = [] + x = pose.x + y = pose.y + theta = np.deg2rad(pose.theta) + comm = np.deg2rad(command) + next_out = ts + + while t < time: + x += sim_ts * v * np.cos(theta) + y += sim_ts * v * np.sin(theta) + theta += sim_ts * (v/self.wheelbase) * np.tan(comm) + + t += sim_ts + if t >= next_out: + output.append((x, y, np.rad2deg(theta))) + next_out += ts + + return output \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py b/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py index ab935c96..81f48a2f 100755 --- a/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py +++ b/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py @@ -21,16 +21,16 @@ class PurePursuitController(Controller): MIN_LOOK_AHEAD_DIST = 0.5 MAX_LOOK_AHEAD_DIST = 10 - def __init__(self, start_index=0) -> None: - super(PurePursuitController, self).__init__(start_index) + def __init__(self, buggy_name, start_index=0) -> None: + super(PurePursuitController, self).__init__(start_index, buggy_name) self.debug_reference_pos_publisher = rospy.Publisher( - "auton/debug/reference_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) self.debug_track_pos_publisher = rospy.Publisher( - "auton/debug/track_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/track_navsat", NavSatFix, queue_size=1 ) self.debug_error_publisher = rospy.Publisher( - "auton/debug/error", ROSPose, queue_size=1 + buggy_name + "/auton/debug/error", ROSPose, queue_size=1 ) def compute_control( diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py index 24a946d0..f2138a33 100644 --- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py @@ -25,13 +25,13 @@ class StanleyController(Controller): CROSS_TRACK_GAIN = 1 HEADING_GAIN = 0.75 - def __init__(self, start_index=0) -> None: - super(StanleyController, self).__init__(start_index) + def __init__(self, buggy_name, start_index=0) -> None: + super(StanleyController, self).__init__(start_index, buggy_name) self.debug_reference_pos_publisher = rospy.Publisher( - "auton/debug/reference_navsat", NavSatFix, queue_size=1 + buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) self.debug_error_publisher = rospy.Publisher( - "auton/debug/error", ROSPose, queue_size=1 + buggy_name + "/auton/debug/error", ROSPose, queue_size=1 ) def compute_control(