Sentry merge

.gitignore

@@ -1,3 +1,5 @@
 .vscode/
 .idea/
 log/
+build/
+install/

.vscode/settings.json

@@ -74,5 +74,6 @@
         "numbers": "cpp",
         "semaphore": "cpp",
         "stop_token": "cpp"
-    }
+    },
+    "cmake.sourceDirectory": "/home/purduerm/ros2-ws/auto-aiming/src/prm_autobot_2023"
 }

README.md

@@ -3,27 +3,38 @@ Revitalized auto-aiming suite for Purdue RoboMaster Club 2024-2025.
 
 ![build status](https://github.com/RoboMaster-Club/auto-aiming/actions/workflows/colcon-tests.yml/badge.svg)
 
-## Usage  
-Clone this repository into your `ros2-ws` directory. We provide a `run` script that can be used to build, run, test, and clean the workspace with no hassle. The script supports the following functionalities:
+## Brief Overview of the Suite
+### Auto-Aim
+Our auto-aiming system uses an industrial camera to detect enemy robots and launch shots at them accurately. Built in ROS2, the system is modular, with each step in the pipeline handled by separate nodes. First, it uses traditional computer vision techniques to detect an enemy robot’s armor plate, and uses `solvepnp` to estimate its 3D position relative to our camera. A ballistics solver then calculates the yaw and pitch angles needed to hit the target. Basic filtering is applied to reduce false positives, and the final aiming commands are sent to the robot's control board over UART.
+
+### Navigation
+Our navigation system enables the Sentry robot to autonomously move around the RoboMaster field, localize itself, and reach any spot on the map. Built using ROS2’s Nav2 framework, it combines LiDAR data and wheel odometry to localize via a method called AMCL. A custom behavior script `subscriber.py` controls where and when the robot moves, while the path planning plugin (DWB) computes obstacle-avoiding paths and issues movement commands. 
+
+### For more details on workings, issues, and possible improvements, see the [State of the Algorithm](https://docs.google.com/document/d/16-y6u_inBcsI0dOPOdocxniNRPu5UtawPmoyKMHaoPU/edit?usp=sharing)
+
+## Installation and Usage
+Clone this repository into your `~/ros2-ws` directory. We provide a `run` script that can be used to build, run, test, and clean the workspace with no hassle. The script supports the following functionalities:
 
 - **Building**
   - `./auto-aiming/run build`
-- **Launch ROS2 code**
+- **Run ROS2 code**
   - `./auto-aiming/run run <launch_file>`
-  - "launch files" basically run several ROS2 nodes at once. The main one is `video2detector.py`.
+    - "launch files" are how you start up the pipeline. For example you may use `video2detector.py` to run the auto-aiming pipeline with a video file, or `mv2control.py` to run using a real camera and send results to the STM32 control board.
 - **Run automated tests (GTest)**
   - `./auto-aiming/run test`
 - **Clean the workspace (remove build and install folders)**
   - `./auto-aiming/run clean`
 - _Optional flags_
   - `--quiet`: Suppresses console output, logs output to `command_output.log`.
-  - `--debug`: Builds with debug flags enabled. Used to display a detection results window and debug logs.
+  - `--debug`: Builds with debug flags enabled. When enabled, displays a detection results window and debug logs.
 
 ### Example to run the detector:
 ```
 ./auto-aiming/run --debug --quiet run video2detector.py
 ```
 
+### To run the navigation stack, please see the README in the `src/prm_autobot_2023` directory.
+
 ## Overall Suite Requirements
 ### Functional Requirements:
 - [x] **Detect an enemy armor plate in the camera's FOV.**

aim_on_boot.sh

@@ -1,73 +1,101 @@
 #!/bin/bash
 
-# Automatically get the current user's username
 SERVICE_NAME="ros2_aiming_service"
 SERVICE_FILE="/etc/systemd/system/$SERVICE_NAME.service"
 
-# Create the auto_aim_logs directory if it doesn't exist
-LOG_DIR="/home/purduerm/auto_aim_logs"
+# Paths
+USER_HOME="/home/jason"
+LOG_DIR="$USER_HOME/auto_aim_logs"
+WRAPPER_SCRIPT="$USER_HOME/start_auto_aim.sh"
+
+# Create log directory if it doesn't exist
 mkdir -p "$LOG_DIR"
 
-# Generate the log file name with current timestamp
+# Create the wrapper script
+create_wrapper_script() {
+    echo "Creating the wrapper script at $WRAPPER_SCRIPT..."
+
+    cat << 'EOF' > "$WRAPPER_SCRIPT"
+#!/bin/bash
+
+# Setup ROS environment
+# source /home/jason/ros2-ws/install/setup.bash
+
+source /opt/ros/humble/setup.bash
+source /home/jason/ros2-ws/install/setup.sh
+
+# Sleep to give system time to stabilize
+sleep 5
+
+# Create log directory and log file
+LOG_DIR="/home/jason/auto_aim_logs"
+mkdir -p "$LOG_DIR"
 LOG_FILE="$LOG_DIR/$(date +'%Y-%m-%d_%H-%M-%S')_auto_aim_log.txt"
 
-# Command to run the service
-COMMANDS_TO_RUN="source /opt/ros/humble/setup.bash && source /home/purduerm/ros2-ws/install/setup.bash && cd /home/purduerm/ros2-ws && ./auto-aiming/run run mv2control.py > $LOG_FILE 2>&1"
+# Change to workspace and exec Python script
+cd /home/jason/ros2-ws
+exec ros2 launch prm_launch mv2control.py >> "$LOG_FILE" 2>&1
+EOF
+
+    chmod +x "$WRAPPER_SCRIPT"
+    echo "Wrapper script created and made executable."
+}
 
-# Function to create the service
+# Create the systemd service unit file
 create_service() {
-    echo "Creating the systemd service..."
+    echo "Creating the systemd service at $SERVICE_FILE..."
 
-    # Create the systemd service unit file
-    cat <<EOL | sudo tee $SERVICE_FILE > /dev/null
+    cat <<EOL | sudo tee "$SERVICE_FILE" > /dev/null
 [Unit]
 Description=ROS 2 Auto Aiming Service
 After=network.target
 
 [Service]
-ExecStart=/bin/bash -c "$COMMANDS_TO_RUN"
-WorkingDirectory=/home/purduerm/ros2-ws
-User=purduerm
-Group=purduerm
+ExecStart=$WRAPPER_SCRIPT
+WorkingDirectory=$USER_HOME/ros2-ws
+User=jason
+Group=jason
 Restart=always
 Environment=ROS_DISTRO=humble
-Environment=HOME=/home/purduerm
+Environment=HOME=$USER_HOME
+StandardOutput=append:$LOG_DIR/systemd_service.log
+StandardError=append:$LOG_DIR/systemd_service.log
 
 [Install]
 WantedBy=multi-user.target
 EOL
 
-    # Reload systemd to recognize the new service
     sudo systemctl daemon-reload
+    echo "Systemd service created and daemon reloaded."
 }
 
-# Function to enable the service to run on boot
+# Enable and start the service
 enable_service() {
-    echo "Enabling the service to start on boot..."
-    sudo systemctl enable $SERVICE_NAME.service
-    sudo systemctl start $SERVICE_NAME.service
+    echo "Enabling and starting the service..."
+    sudo systemctl enable "$SERVICE_NAME.service"
+    sudo systemctl start "$SERVICE_NAME.service"
     echo "Service started and enabled."
 }
 
-# Function to disable and remove the service
+# Disable and remove the service
 disable_service() {
     if [ -f "$SERVICE_FILE" ]; then
-        echo "Disabling and removing the service..."
-        sudo systemctl stop $SERVICE_NAME.service
-        sudo systemctl disable $SERVICE_NAME.service
-        sudo rm $SERVICE_FILE
+        echo "Stopping and removing the service..."
+        sudo systemctl stop "$SERVICE_NAME.service"
+        sudo systemctl disable "$SERVICE_NAME.service"
+        sudo rm "$SERVICE_FILE"
         sudo systemctl daemon-reload
         echo "Service stopped, disabled, and removed."
     else
         echo "Service does not exist. Nothing to disable."
     fi
 }
 
-# Parse arguments
+# Main logic: parse arguments
 if [[ "$1" == "--disable" ]]; then
     disable_service
 else
-    # Create and enable the service by default
+    create_wrapper_script
     create_service
     enable_service
 fi

run

@@ -28,6 +28,70 @@ print_blue "        \__/  <__>  <__>  <__>  <__>       "
 print_blue "                                           "
 print_blue "-------------------------------------------"
 
+# Function to check if MindVision camera is attached and on USB 3.0
+check_mindvision_camera() {
+    echo "Checking for MindVision camera..."
+    local retries=0
+    local max_retries=3
+
+    while (( retries < max_retries )); do
+        success_count=0
+        for i in {1..20}; do
+            if lsusb | grep -q "MindVision"; then
+                ((success_count++))
+            else
+                success_count=0
+            fi
+            sleep 0.2
+        done
+
+        if [[ $success_count -eq 20 ]]; then
+            # Ensure camera is USB 3.0
+            bus=$(lsusb | grep "MindVision" | awk '{print $2}' | sed 's/^0*//')
+            speed=$(lsusb -t | grep "Bus 0*$bus" | grep -oP ', \K[0-9]+[MGK]?' | head -n1)
+            speed_value=$(echo "$speed" | sed 's/[^0-9]*//g')
+
+            echo "MindVision camera detected on bus $bus with speed $speed."
+
+            if [[ "$speed_value" -lt 5000 ]]; then
+                print_red "[!] WARN: MindVision camera is not connected to a USB 3.0 bus! Reloading USB Drivers... (attempt $((retries+1))/$max_retries)"
+                ((retries++))
+                rebind_usb
+                continue
+            else
+                print_green "[☺] MindVision camera detected on USB 3.0 bus [$bus] with speed ${speed}."
+                return 0
+            fi
+        else
+            print_red "[!] WARN: MindVision camera not detected (attempt $((retries+1))/$max_retries)"
+            ((retries++))
+            rebind_usb
+        fi
+    done
+
+    print_red "[!] ERROR: MindVision camera not detected or not on USB 3.0 after $max_retries attempts."
+    return 1
+}
+
+rebind_usb() {
+    # Get all USB controller PCI IDs
+    usb_ids=$(lspci | grep -i usb | awk '{print "0000:" $1}')
+
+    # Unbind all USB controllers
+    for id in $usb_ids; do
+        echo "Unbinding $id"
+        echo "purdueRM2023" | sudo -S echo -n "$id" | sudo tee "/sys/bus/pci/devices/$id/driver/unbind" > /dev/null
+    done
+
+    sleep 0.5
+
+    # Rebind all USB controllers
+    for id in $usb_ids; do
+        echo "Rebinding $id"
+        echo "purdueRM2023" | sudo -S echo -n "$id" | sudo tee "/sys/bus/pci/drivers/xhci_hcd/bind" > /dev/null
+    done
+}
+
 # Function to clean the workspace
 clean() {
     print_green "Cleaning workspace..."
@@ -39,7 +103,8 @@ clean() {
 build() {
     print_green "[*] Building project with Release configuration and optimization flags."
 
-    build_args="-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS_RELEASE=-O3"
+    # Last two args needed to correctly build the livox lidar driver
+    build_args="-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS_RELEASE=-O3 -DROS_EDITION=ROS2 -DHUMBLE_ROS=humble"
 
     # Add the DEBUG flag if '--debug' or '-d' was provided
     if [[ "$debug" == "true" ]]; then
@@ -54,7 +119,8 @@ build() {
 
     if [ $? -eq 0 ]; then
         # print checkmark
-        print_green "[☺ ] Build successful."
+        print_green "[☺] Build successful."
+        sleep 0.2
         source install/setup.sh
     else
         print_red "Build failed. See $LOG_FILE for more information."
@@ -121,8 +187,11 @@ run() {
 
     launch_file="$1"
 
-    # Run build first
-    build
+    # # Run build first
+    # build
+
+    # Ensure Camera is connected
+    check_mindvision_camera
 
     print_green "[*] Running ROS2 launch: prm_launch $launch_file"
     ros2 launch prm_launch "$launch_file"

src/livox_ros_driver2/.gitignore

@@ -0,0 +1,4 @@
+.vscode
+build
+package.xml
+__pycache__