Merge branch 'main' into mehulg/ekfImplementation

# Conflicts:
#	docker_auton/Dockerfile

README.md

@@ -1,73 +1,95 @@
 # RoboBuggy2
-A complete re-write of the old RoboBuggy.
+A complete re-write of the old RoboBuggy. This code was run for RD23, RD24 and RD25, on both NAND and Short Circuit.
+
 
----
 ## Table of Contents
- - Installation
- - Quickstart
- - Development
+ - Installation and Initial Setup
+ - Launching Code
+ - Infrastructure Documentation
+ - Code Structure and Documentation
 
 
 ---
-## 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/.
+## Installation and Initial Setup
+### Necessary + Recommended Software
+- Docker
+- Foxglove
+- VSCode (recommended)
+- Git (recommended)
+
 
 ### Docker
-- You will need [Docker](https://docs.docker.com/get-docker/) installed.
+- Installation instructions here: https://docs.docker.com/get-docker/
+
+### Foxglove
+- Installation instructions here: https://foxglove.dev/
+
+### VSCode
+- https://code.visualstudio.com/download
+
+### Git
+- https://git-scm.com/downloads
+
+### Install Softwares: WSL, Ubuntu (Windows only)
+- Go to Microsoft Store to install "Ubuntu 20.04.6 LTS".
 
 ### 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).
 
-### 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)
 
-
-### 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.
-
-## 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.
+### Apple Silicon Mac Only:
+- In Docker Desktop App: go to settings -> general and turn on "Use Rosetta for x86/amd64 emulation on Apple Silicon"
 
 ### 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
+- Note: Ensure that the SSH keys are generated while in the 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).
 
-### Clone
-- In your terminal type: `$ git clone [email protected]: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)
 
+### Clone the Repository
+This is so you can edit our codebase locally, and sync your changes with the rest of the team through Git.
+- In your terminal type: `$ git clone https://github.com/CMU-Robotics-Club/RoboBuggy2.git`.
+- The clone link above is the URL or can be found above: code -> local -> Clone HTTPS.
 
-### 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.
+### 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)
 
----
-## Open Docker
+
+
+### X11 Setup (recommended)
+- 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.
+- Windows: Make sure that you're using WSL 2 Ubuntu and NOT command prompt.
+- 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.
+
+
+## Launching Code
+### 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
+### ROS
+- Navigate to `/rb_ws`. This is the catkin workspace where we will be doing all our ROS stuff.
+- (This should only need to be run the first time you set up the repository) - 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.
+
+### 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
@@ -80,38 +102,8 @@ A complete re-write of the old RoboBuggy.
 - 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.
-- Windows: Make sure that you're using WSL 2 Ubuntu and NOT command prompt.
-- 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.
-
----
-### Connecting to and Launching the RoboBuggy
-When launching the buggy:
+### Connecting to and Launching the RoboBuggies
+When launching Short Circuit:
 - Connect to the Wi-Fi named ShortCircuit.
 -	In the command line window:
 SSH to the computer on ShortCircuit and go to folder
@@ -122,13 +114,31 @@ Then `$ cd RoboBuggy2`
 -	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 sc-main.launch` (or `$ roslaunch buggy nand-main.launch` for NAND)
+-	Roslauch in docker container by `$ roslaunch buggy sc-main.launch`
 (wait until no longer prints “waiting for covariance to be better”)
 
+When launching NAND:
+- Ask software lead (WIP)
+
 When shutting down the buggy:
 -	Stop roslauch
 `$ ^C  (Ctrl+C)`
 -	Leave the docker container
 `$ exit`
 -	Shutdown the ShortCircuit computer
 `$ sudo shutdown now`
+
+## Documentation
+### Infrastructure Documentation
+Ask Software Lead (WIP)
+
+### Simulator notes
+- Longitude + Latitude for Foxglove visualization on map: `/state/pose_navsat` (sensor_msgs/NavSatFix)
+- UTM coordinates (assume we're in Zone 17N): `/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)
+
+### Auton Logic
+Ask someone with experience (WIP)

camera.py

@@ -0,0 +1,70 @@
+########################################################################
+#
+# Copyright (c) 2022, STEREOLABS.
+#
+# All rights reserved.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+########################################################################
+
+import sys
+from signal import signal, SIGINT
+import argparse
+
+
+import pyzed.sl as sl
+
+cam = sl.Camera()
+
+#Handler to deal with CTRL+C properly
+def handler(signal_received, frame):
+    cam.disable_recording()
+    cam.close()
+    sys.exit(0)
+
+signal(SIGINT, handler)
+
+def main():
+
+    init = sl.InitParameters()
+    init.depth_mode = sl.DEPTH_MODE.NONE # Set configuration parameters for the ZED
+
+    status = cam.open(init)
+    if status != sl.ERROR_CODE.SUCCESS:
+        print("Camera Open", status, "Exit program.")
+        exit(1)
+
+    recording_param = sl.RecordingParameters(opt.output_svo_file, sl.SVO_COMPRESSION_MODE.H264) # Enable recording with the filename specified in argument
+    err = cam.enable_recording(recording_param)
+    if err != sl.ERROR_CODE.SUCCESS:
+        print("Recording ZED : ", err)
+        exit(1)
+
+    runtime = sl.RuntimeParameters()
+    print("SVO is Recording, use Ctrl-C to stop.") # Start recording SVO, stop with Ctrl-C command
+    frames_recorded = 0
+
+    while True:
+        if cam.grab(runtime) == sl.ERROR_CODE.SUCCESS : # Check that a new image is successfully acquired
+            frames_recorded += 1
+            print("Frame count: " + str(frames_recorded), end="\r")
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--output_svo_file', type=str, help='Path to the SVO file that will be written', required= True)
+    opt = parser.parse_args()
+    if not opt.output_svo_file.endswith(".svo") and not opt.output_svo_file.endswith(".svo2"):
+        print("--output_svo_file parameter should be a .svo file but is not : ",opt.output_svo_file,"Exit program.")
+        exit()
+    main()

docker_auton/Dockerfile

@@ -1,8 +1,8 @@
-FROM nvidia/cuda:11.6.2-base-ubuntu20.04 as CUDA
+# FROM nvidia/cuda:11.6.2-base-ubuntu20.04 as CUDA
 
 FROM osrf/ros:noetic-desktop-full-focal
 
-COPY --from=CUDA /usr/local/cuda /usr/local/
+# COPY --from=CUDA /usr/local/cuda /usr/local/
 
 
 RUN apt update
@@ -18,10 +18,11 @@ RUN apt-get install -y -qq \
   ros-noetic-microstrain-inertial-driver \
   ros-noetic-realsense2-camera \
   ros-noetic-realsense2-description \
-  ros-noetic-robot-pose-ekf
+  ros-noetic-robot-pose-ekf \
+  ros-${ROS_DISTRO}-mavros ros-${ROS_DISTRO}-mavros-extras ros-${ROS_DISTRO}-mavros-msgs
 
 # Run this now to cache it separately from other requirements
-COPY cuda-requirements_TEMP_DO_NOT_EDIT.txt cuda-requirements.txt
+# COPY cuda-requirements_TEMP_DO_NOT_EDIT.txt cuda-requirements.txt
 # RUN pip3 install -r cuda-requirements.txt
 
 

docs/architecture/node_interactions.md

@@ -0,0 +1,64 @@
+# ROS Node Interaction Document
+## Scope
+The purpose of this file is to descirbe the architecture of the RD25 stack from a ROS node level. This document should be updated whenever any ROS topics are added, removed or modified.
+
+## Viewing On VSCode
+Install `bierner.markdown-mermaid` from the extension marketplace to render the charts in VSCode markdown preview.
+
+## List of topics
+| Topic Name | Type                    | is custom message  |
+| ---------- | ----------------------- | ------------------ |
+| ego/state  | BuggyState              | yes |
+| other_buggy/state | BuggyState       | yes |
+| ego/trajectory | BuggyTrajectory     | yes |
+| ego/steering_cmd | [Float64](https://docs.ros.org/en/melodic/api/std_msgs/html/msg/Float64.html) | no |
+| add new topic here | | |
+
+## ROS Nodes Graph
+Auton Loop [Sequence Diagram](https://en.wikipedia.org/wiki/Sequence_diagram#:~:text=A%20sequence%20diagram%20shows%2C%20as,order%20in%20which%20they%20occur.)
+
+```mermaid
+sequenceDiagram
+A->>B: "some/topic"
+```
+
+means A publishes a message of `some/topic` and B receives the message.
+
+```mermaid
+sequenceDiagram
+    participant I as Ego State Estimator Node
+    participant P as Planner Node
+    participant C as Controller Node
+    participant S as Firmware Comms Node
+
+    box Grey HBK 3DM-GQ7 INS
+        participant I
+    end
+
+    box Purple Auton Core
+        participant P
+        participant C
+    end
+
+    box Blue Communication with Sensors, Actuators and Other Buggy
+        participant S
+    end
+
+    loop 100hz
+        I->>P: "ego/state"
+        I->>C: "ego/state"
+    end
+
+    loop 10hz
+        S->>P: "other_buggy/state"
+    end
+
+    loop 10hz
+        P->>C: "ego/trajectory"
+    end
+
+    loop 100hz
+        C->>S: "ego/steering_cmd"
+    end
+```
+

python-requirements.txt

@@ -1,4 +1,5 @@
 matplotlib==3.1.2
+NavPy==1.0
 numba==0.58.0
 numpy<1.21.0
 osqp==0.6.3