From bffdd173699e1b8454ff7b3f33e652b2c47f0220 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 28 Apr 2026 15:28:28 -0700 Subject: [PATCH 1/5] Add GitHub CI, apply polymath_code_standard --- .github/workflows/build.yml | 29 ++ .gitignore | 2 +- .gitlab-ci.yml | 29 -- .pre-commit-config.yaml | 21 ++ LICENSE | 2 +- README.md | 4 - archive/docs/Case.md | 2 +- archive/docs/Hardware.md | 31 +- archive/docs/Robot-Quickstart.md | 4 +- archive/docs/Software.md | 12 +- .../protective_stop_remote/app.py | 156 +++++----- .../protective_stop_remote/button.py | 44 +-- .../protective_stop_remote/main.py | 16 +- .../protective_stop_remote/power.py | 62 ++-- .../protective_stop_remote/pstop.py | 278 +++++++++--------- .../templates/index.html | 13 +- .../protective_stop_remote/ui.py | 103 ++++--- archive/protective_stop_remote/rosdep.yaml | 13 +- archive/protective_stop_remote/setup.py | 26 +- .../protective_stop_remote/tests/test_app.py | 4 +- .../launch/protective_stop_node.launch.yaml | 22 +- .../protective_stop_node/models.py | 44 ++- .../protective_stop_node.py | 228 ++++++-------- .../test_utils/__init__.py | 1 - .../test_utils/base_test.py | 31 +- .../test_utils/helpers.py | 12 +- protective_stop_node/test/launch.test.py | 102 +++---- .../test/unmonitored_mode.test.py | 44 ++- protective_stop_remote_example/channel.json | 2 +- .../public/vite.svg | 2 +- .../src/ProtectiveStopMsgSchema.txt | 1 - .../src/ProtectiveStopSrvResponseSchema.txt | 1 - .../src/assets/react.svg | 2 +- .../src/test/ws-server.ts | 2 +- pstop_c/CMakeLists.txt | 1 - pstop_c/pstop/include/pstop/pstop_msg.h | 1 - pstop_c/pstop/include/pstop/time.h | 1 - pstop_c/pstop/src/pstop/time.c | 1 - pstop_c/pstop/test/src/pstop/machine_test.c | 2 +- .../test/src/pstop/machine_timeout_test.c | 2 +- pstop_c/pstop/test/src/pstop/main.c | 2 +- .../pstop/test/src/pstop/pstop_client_test.c | 2 +- pstop_c/pstop/test/src/pstop/pstop_tests.yml | 3 +- pstop_c/pstop/test/src/pstop/test_utils.c | 1 - 44 files changed, 640 insertions(+), 721 deletions(-) create mode 100644 .github/workflows/build.yml delete mode 100644 .gitlab-ci.yml create mode 100644 .pre-commit-config.yaml diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..93855d8 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,29 @@ +--- +name: ROS2 Build and Test + +on: + push: + branches: [main] + pull_request: + +jobs: + build_and_test_ros2: + runs-on: ubuntu-latest + strategy: + matrix: + include: + - rosdistro: humble + ubuntu: jammy + - rosdistro: jazzy + ubuntu: noble + - rosdistro: kilted + ubuntu: noble + - rosdistro: rolling + ubuntu: noble + container: + image: rostooling/setup-ros-docker:ubuntu-${{ matrix.ubuntu }}-latest + steps: + - name: Build and run tests + uses: ros-tooling/action-ros-ci@v0.4 + with: + target-ros2-distro: ${{ matrix.rosdistro }} diff --git a/.gitignore b/.gitignore index d213fd2..3cf700d 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,4 @@ __pycache__ .pytest_cache node_modules -/.gitlab-ci-local/ +/.ruff.toml diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml deleted file mode 100644 index de3be01..0000000 --- a/.gitlab-ci.yml +++ /dev/null @@ -1,29 +0,0 @@ ---- - -stages: - - build - - test - - evaluate - -workflow: - rules: - - if: $CI_COMMIT_BRANCH == $CI_DEFAULT_BRANCH - variables: - DEFAULT_VCS_CHECKOUT: "" - - if: $CI_COMMIT_TAG - variables: - DEFAULT_VCS_CHECKOUT: "" - - if: $CI_PIPELINE_SOURCE == 'merge_request_event' - variables: - DEFAULT_VCS_CHECKOUT: $CI_COMMIT_REF_NAME - -include: - - component: gitlab.com/polymathrobotics/polymath_core/ros2-package@ci-1.3 - inputs: - base_image: docker.io/polymathrobotics/ros:humble-builder-ubuntu - -# Build all packages in the workspace and run their test suites -ros2_build_and_test: - extends: .ros2_build_and_test - variables: - PACKAGES: "" diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..0ad13cc --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,21 @@ +--- +# Apply Polymath Code Standard formatters and linters +repos: + - repo: git@github.com:polymathrobotics/polymath_code_standard.git + rev: v2.1.0 + hooks: + # Everything + - id: polymath-general + - id: polymath-copyright + args: [--license, Apache-2.0, --wildcard-copyright-org, --reuse-style] + # Implementation + - id: polymath-cmake + - id: polymath-cpp + exclude: ^pstop_c/ + - id: polymath-json + - id: polymath-python + # Mark(up/down) + - id: polymath-markdown + - id: polymath-shell + - id: polymath-xml + - id: polymath-yaml diff --git a/LICENSE b/LICENSE index 2f80464..ea2db4f 100644 --- a/LICENSE +++ b/LICENSE @@ -186,7 +186,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright [2024] [Polymath Robotics, Inc.] + Copyright 2024 Polymath Robotics, Inc. Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. diff --git a/README.md b/README.md index d5ae582..bee4fb3 100644 --- a/README.md +++ b/README.md @@ -42,7 +42,6 @@ To register a remote protective stop, the node also exposes two services: In order to integrate with the node, you have to activate it first before starting to send heartbeat messages. - #### User Monitor Mode **USE WITH EXTREME CAUTION** @@ -53,14 +52,12 @@ In the case you have a test driver or other onsite personnel to operate a physic ros2 param set /protective_stop_node is_user_monitored False ``` - ### Protective Stop Remote The remote is run next to the operator of the P-Stop. It exchanges heartbeat messages with the node on the robot, the latter of which will signal if it detects that the heartbeats are coming in at the expected rate. In this repo currently, we've implemented it as a web client, but it can also be instantiated as a hardware interface. - ### Foxglove Websocket Bridge The bridge acts as the proxy between the remote and the node, so the two can talk to one-another. If it is not online, the other components will fail gracefully. @@ -73,4 +70,3 @@ cd build cmake .. make ``` - diff --git a/archive/docs/Case.md b/archive/docs/Case.md index 926304b..84dd5c0 100644 --- a/archive/docs/Case.md +++ b/archive/docs/Case.md @@ -1 +1 @@ -Instructions to be added \ No newline at end of file +Instructions to be added diff --git a/archive/docs/Hardware.md b/archive/docs/Hardware.md index 4941e89..61a7705 100644 --- a/archive/docs/Hardware.md +++ b/archive/docs/Hardware.md @@ -24,8 +24,7 @@

- -2. Solder Raspberry Pi GPIO pin extender +1. Solder Raspberry Pi GPIO pin extender - Use flux to make soldering easier - Secure a few pins on each end first for better stability - May want to clean singe marks from flux after soldering; we used a toothbrush and water for this. @@ -33,8 +32,7 @@
Left = before, right = after

- -3. Apply Raspberry Pi heat sink. +1. Apply Raspberry Pi heat sink. - The Geekworm heatsink we used came with two thermal pads, one is 0.5mm and the other is 1mm. Make sure each pad goes in the right place! It comes with a diagram.


Before @@ -49,8 +47,7 @@
Thermal Pad Placement

- -4. Add e-ink display to UPS using GPIO header. +1. Add e-ink display to UPS using GPIO header.


Top to bottom: UPS, GPIO header, e-ink display

@@ -61,16 +58,16 @@
Install e-ink to UPS by plugging in GPIO header to pins

- ## Wiring Now it’s time to starting connecting stuff together. The connections at a glance are shown below. + ```mermaid graph TD USB-C -->|Power Out| UPS(UPS) UPS(UPS) -->|Power Out| R(Raspi) R -->|Control| ES(Eink Screen) R <-->|USB Power and Data| C(Cell Modem) - C -->|USB Power and Data| E(ESP32) + C -->|USB Power and Data| E(ESP32) E -->|D2 to DI, + and GND| L(LED Ring) E -->|D22 to switch, + and GNDx2| PB(Power Button) PB -->|switch to input| UPS @@ -99,7 +96,7 @@ We will go through the wire soldering connections in the diagram above one by on
Corresponding ESP32 wiring for stop button

-2. LED Ring to ESP32 +1. LED Ring to ESP32 - LED Ring DI → ESP32 D2 (Green wire) - LED Ring PWR5V → ESP32 VIN (Red wire) - LED Ring GND → ESP32 GND (Black wire) @@ -111,7 +108,7 @@ We will go through the wire soldering connections in the diagram above one by on
Corresponding ESP32 wiring for LED ring

-3. Power Button to ESP32 and UPS +1. Power Button to ESP32 and UPS - Power Button LED power terminal → ESP32 D22 and UPS pin 3 (White wires) - Power Button LED ground and switch contact A → ESP32 GND (Black wire) - Power Button switch contact B→ ESP32 D23 (Red wire) @@ -134,12 +131,12 @@ We will go through the wire soldering connections in the diagram above one by on

- Two white wires should be connected to the same terminal on the button (power button LED power terminal) - - Tip: Solder these wires together first, and then solder them to the terminal + - Tip: Solder these wires together first, and then solder them to the terminal - The black wire is soldered across two power button terminals (power button LED ground and switch contact A) - - Tip: Solder the end of the wire to the further terminal first, then the closer one + - Tip: Solder the end of the wire to the further terminal first, then the closer one - Red wire is soldered normally -4. UPS to ESP32 (battery indicator) +1. UPS to ESP32 (battery indicator) - Lowbat to D34 (yellow wire)


UPS lowbat soldering @@ -148,16 +145,16 @@ We will go through the wire soldering connections in the diagram above one by on
ESP32 pin D34 soldering

-5. Connect cell modem to ESP32 and RPi UPS. +1. Connect cell modem to ESP32 and RPi UPS. - Modem USB → ESP32 microUSB (solid black cable) - Modem microUSB → UPS microUSB (black/white cable) -6. Add battery to USP. +1. Add battery to USP.


Wired and connected system.

-7. Add antennas - if you want to use cellular data instead of WiFi +1. Add antennas - if you want to use cellular data instead of WiFi - Screw on antennas to AUX and MAIN pins on the underside of the modem -Now you're ready to move onto [Software](/docs/Software.md) \ No newline at end of file +Now you're ready to move onto [Software](/docs/Software.md) diff --git a/archive/docs/Robot-Quickstart.md b/archive/docs/Robot-Quickstart.md index 141560f..cee2495 100644 --- a/archive/docs/Robot-Quickstart.md +++ b/archive/docs/Robot-Quickstart.md @@ -33,7 +33,7 @@ source install/setup.bash In a terminal on your local machine where you have built and sourced the custom message type above: 1. Launch a rosbridge_server with `ros2 launch rosbridge_server rosbridge_websocket_launch.xml`. - If you are running this in a Docker container, make sure that port 9090 is exposed by doing `docker run -p 9090:9090 ...` -2. To run the roslibpy client, run `python roslibpy_client.py [ip address]`. +2. To run the roslibpy client, run `python roslibpy_client.py [ip address]`. - You can set a default ip address by altering line 12 of [`roslibpy_client.py`](../roslibpy_client.py): ``` parser.add_argument('target', type=str, help='Target IP address', default='') @@ -42,4 +42,4 @@ parser.add_argument('target', type=str, help='Target IP address', default='') ``` ssh -L 8000:localhost:5000 [username]@[tailscale ip] ``` -- Now, you should be able to see the flask interface by going to `http://localhost:8000/config`. \ No newline at end of file +- Now, you should be able to see the flask interface by going to `http://localhost:8000/config`. diff --git a/archive/docs/Software.md b/archive/docs/Software.md index e2b2bfb..447d31e 100644 --- a/archive/docs/Software.md +++ b/archive/docs/Software.md @@ -29,7 +29,7 @@ 4. Proceed with flashing the Pi — there will be a “write” and “verify” step. -5. Remove microSD from your computer and insert it in the Raspberry Pi. Turn on power and wait a couple minutes for the Raspberry Pi to boot. +5. Remove microSD from your computer and insert it in the Raspberry Pi. Turn on power and wait a couple minutes for the Raspberry Pi to boot. 6. You can use find the IP address of the Raspberry Pi by going on your computer and running `nmap -p 22 --open [your network range]`. - Your network range will be something like `192.168.93.0/24`, and can be found by running `ifconfig` or `ip addr`. @@ -52,9 +52,9 @@ sudo tailscale up 3. Install the following packages on the Raspberry Pi: - Install chrony with `sudo apt install chrony` -- Install pip with `sudo apt install python3-pip -y` +- Install pip with `sudo apt install python3-pip -y` - Install PySerial with `sudo pip install pyserial --break-system-packages` -- Install the roslibpy with: +- Install the roslibpy with: ``` sudo apt install git-all git clone https://github.com/gramaziokohler/roslibpy.git @@ -161,7 +161,7 @@ sudo pip3 install spidev 3. Go in the Raspberry Pi directory with: `cd e-Paper/RaspberryPi_JetsonNano/` 4. Set up the libraries with `sudo python3 [setup.py](http://setup.py/) install` 5. Install flask with `pip install flask --break-system-packages` -6. Configure the Raspberry Pi by typing `sudo raspi-config`. +6. Configure the Raspberry Pi by typing `sudo raspi-config`. - Go to `Choose Interfacing Options -> SPI -> Yes Enable SPI interface`


Raspberry Pi Config Menu @@ -190,7 +190,7 @@ source install/setup.bash In a terminal on your local machine where you have built and sourced the custom message type above: 1. Launch a rosbridge_server with `ros2 launch rosbridge_server rosbridge_websocket_launch.xml`. - If you are running this in a Docker container, make sure that port 9090 is exposed by doing `docker run -p 9090:9090 ...` -2. To run the roslibpy client, run `python roslibpy_client.py [ip address]`. +2. To run the roslibpy client, run `python roslibpy_client.py [ip address]`. - You can set a default ip address by altering line 12 of [`roslibpy_client.py`](../roslibpy_client.py): ``` parser.add_argument('target', type=str, help='Target IP address', default='') @@ -222,4 +222,4 @@ WantedBy=multi-user.target 4. Enable the service: `sudo systemctl enable pstop-autostart.service`. 5. This service should now start on boot! - You can reboot either by power cycling or with `sudo reboot`. -- You can also test it without rebooting with `sudo systemctl start pstop-autostart.service`. \ No newline at end of file +- You can also test it without rebooting with `sudo systemctl start pstop-autostart.service`. diff --git a/archive/protective_stop_remote/protective_stop_remote/app.py b/archive/protective_stop_remote/protective_stop_remote/app.py index 5d3bda1..34f9597 100644 --- a/archive/protective_stop_remote/protective_stop_remote/app.py +++ b/archive/protective_stop_remote/protective_stop_remote/app.py @@ -1,13 +1,10 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - # Handles configuration via webpage + saving config to json file import json -import subprocess import re -from flask import Flask, request, redirect, render_template, jsonify -import logging +import subprocess + +from flask import Flask, jsonify, redirect, render_template, request app = Flask(__name__) CONFIG_FILE = 'config.json' @@ -17,121 +14,118 @@ def list_wifi_connections(): try: - result = subprocess.run(['nmcli', '-t', '-f', 'NAME,DEVICE,STATE', - 'connection', 'show'], capture_output=True, text=True) + result = subprocess.run( + ['nmcli', '-t', '-f', 'NAME,DEVICE,STATE', 'connection', 'show'], capture_output=True, text=True + ) if result.returncode != 0: - raise Exception(f"nmcli error: {result.stderr}") + raise Exception(f'nmcli error: {result.stderr}') connections = [] - for line in result.stdout.strip().split("\n"): + for line in result.stdout.strip().split('\n'): name, device, state = line.split(':') - connections.append( - {"name": name, "device": device, "state": state}) + connections.append({'name': name, 'device': device, 'state': state}) return connections except Exception as e: - print(f"Error listing Wi-Fi connections: {e}") + print(f'Error listing Wi-Fi connections: {e}') return [] def add_wifi_connection(ssid, password): try: - result = subprocess.run(['nmcli', 'dev', 'wifi', 'connect', - ssid, 'password', password], capture_output=True, text=True) + result = subprocess.run( + ['nmcli', 'dev', 'wifi', 'connect', ssid, 'password', password], capture_output=True, text=True + ) if result.returncode != 0: - raise Exception(f"nmcli error: {result.stderr}") + raise Exception(f'nmcli error: {result.stderr}') return True except Exception as e: - print(f"Error adding Wi-Fi connection: {e}") + print(f'Error adding Wi-Fi connection: {e}') return False def remove_wifi_connection(name): try: - result = subprocess.run( - ['nmcli', 'connection', 'delete', name], capture_output=True, text=True) + result = subprocess.run(['nmcli', 'connection', 'delete', name], capture_output=True, text=True) if result.returncode != 0: - raise Exception(f"nmcli error: {result.stderr}") + raise Exception(f'nmcli error: {result.stderr}') return True except Exception as e: - print(f"Error removing Wi-Fi connection: {e}") + print(f'Error removing Wi-Fi connection: {e}') return False def activate_wifi_connection(name): """Activate a specific Wi-Fi connection.""" try: - result = subprocess.run( - ['nmcli', 'connection', 'up', name], capture_output=True, text=True) + result = subprocess.run(['nmcli', 'connection', 'up', name], capture_output=True, text=True) if result.returncode != 0: - raise Exception(f"nmcli error: {result.stderr}") + raise Exception(f'nmcli error: {result.stderr}') return True except Exception as e: - print(f"Error activating Wi-Fi connection: {e}") + print(f'Error activating Wi-Fi connection: {e}') return False + # Tailscale and signal strength functions def get_wifi_signal_strength(): try: - result = subprocess.run( - ['nmcli', '-t', '-f', 'IN-USE,SIGNAL', 'dev', 'wifi'], capture_output=True, text=True) + result = subprocess.run(['nmcli', '-t', '-f', 'IN-USE,SIGNAL', 'dev', 'wifi'], capture_output=True, text=True) if result.returncode != 0: - raise Exception(f"nmcli error: {result.stderr}") + raise Exception(f'nmcli error: {result.stderr}') - wifi_lines = result.stdout.strip().split("\n") + wifi_lines = result.stdout.strip().split('\n') for line in wifi_lines: if line.startswith('*'): _, signal = line.split(':') return int(signal) except Exception as e: - print(f"Error getting Wi-Fi signal strength: {e}") + print(f'Error getting Wi-Fi signal strength: {e}') return None def get_cellular_signal_strength(): try: - result = subprocess.run( - ['mmcli', '-L'], capture_output=True, text=True) + result = subprocess.run(['mmcli', '-L'], capture_output=True, text=True) if result.returncode != 0: - raise Exception(f"mmcli error: {result.stderr}") + raise Exception(f'mmcli error: {result.stderr}') output = result.stdout.strip() match = re.search(r'/Modem/(\d+)', output) if not match: - raise Exception("Could not find a modem index in mmcli output.") + raise Exception('Could not find a modem index in mmcli output.') modem_index = match.group(1) - result = subprocess.run( - ['mmcli', '-m', modem_index], capture_output=True, text=True) + result = subprocess.run(['mmcli', '-m', modem_index], capture_output=True, text=True) if result.returncode != 0: - raise Exception(f"mmcli error: {result.stderr}") + raise Exception(f'mmcli error: {result.stderr}') output = result.stdout signal_match = re.search(r'signal quality: (\d+)', output) if signal_match: return int(signal_match.group(1)) else: - raise Exception("Signal quality not found in modem details.") + raise Exception('Signal quality not found in modem details.') except Exception as e: - print(f"Error getting cellular signal strength: {e}") + print(f'Error getting cellular signal strength: {e}') return None def fetch_tailscale_info(shared): try: - result = subprocess.run( - ["tailscale", "status", "--json"], capture_output=True, text=True, check=True) + result = subprocess.run(['tailscale', 'status', '--json'], capture_output=True, text=True, check=True) status = json.loads(result.stdout) - tailscale_ips = status.get("Self", {}).get("TailscaleIPs", []) - dns_name = status.get("Self", {}).get("DNSName", "") - shared["TailscaleIP"] = tailscale_ips[0] if tailscale_ips else "N/A" - shared["DNSName"] = dns_name if dns_name else "N/A" + tailscale_ips = status.get('Self', {}).get('TailscaleIPs', []) + dns_name = status.get('Self', {}).get('DNSName', '') + shared['TailscaleIP'] = tailscale_ips[0] if tailscale_ips else 'N/A' + shared['DNSName'] = dns_name if dns_name else 'N/A' except Exception as e: - print(f"Error fetching Tailscale info: {e}") - shared["TailscaleIP"] = "N/A" - shared["DNSName"] = "N/A" + print(f'Error fetching Tailscale info: {e}') + shared['TailscaleIP'] = 'N/A' + shared['DNSName'] = 'N/A' + # Load and save configuration @@ -150,31 +144,32 @@ def save_config(cfg): with open(CONFIG_FILE, 'w') as f: json.dump(cfg, f, indent=2) except Exception as e: - print(f"Error saving config: {e}") + print(f'Error saving config: {e}') + # Shared dictionary initialization def initialize_shared_dict(shared): """Ensure all required keys exist in the shared dictionary with default values.""" - if "status_summary" not in shared: - shared["status_summary"] = "Not Connected" - if "uuid" not in shared: - shared["uuid"] = "Unassigned!" - if "TailscaleIP" not in shared: - shared["TailscaleIP"] = "N/A" - if "DNSName" not in shared: - shared["DNSName"] = "N/A" - if "battery_level" not in shared: - shared["battery_level"] = "Unknown" - if "WifiSignal" not in shared: - shared["WifiSignal"] = "N/A" - if "CellularSignal" not in shared: - shared["CellularSignal"] = "N/A" - if "target_pairs" not in shared: - shared["target_pairs"] = [] - if "WifiConnections" not in shared: - shared["WifiConnections"] = [] + if 'status_summary' not in shared: + shared['status_summary'] = 'Not Connected' + if 'uuid' not in shared: + shared['uuid'] = 'Unassigned!' + if 'TailscaleIP' not in shared: + shared['TailscaleIP'] = 'N/A' + if 'DNSName' not in shared: + shared['DNSName'] = 'N/A' + if 'battery_level' not in shared: + shared['battery_level'] = 'Unknown' + if 'WifiSignal' not in shared: + shared['WifiSignal'] = 'N/A' + if 'CellularSignal' not in shared: + shared['CellularSignal'] = 'N/A' + if 'target_pairs' not in shared: + shared['target_pairs'] = [] + if 'WifiConnections' not in shared: + shared['WifiConnections'] = [] def load_shared_from_config(shared): @@ -194,19 +189,19 @@ def config_app(shared={}): def get_data(): """API endpoint for live data updates.""" fetch_tailscale_info(shared) - shared["WifiSignal"] = get_wifi_signal_strength() or "N/A" - shared["CellularSignal"] = get_cellular_signal_strength() or "N/A" - shared["WifiConnections"] = list_wifi_connections() + shared['WifiSignal'] = get_wifi_signal_strength() or 'N/A' + shared['CellularSignal'] = get_cellular_signal_strength() or 'N/A' + shared['WifiConnections'] = list_wifi_connections() return jsonify({ - "status_summary": shared.get("status_summary", "Not Connected"), - "uuid": shared.get("uuid", "Unassigned"), - "TailscaleIP": shared.get("TailscaleIP", "N/A"), - "DNSName": shared.get("DNSName", "N/A"), - "BatteryLevel": shared.get("battery_level", "Unknown"), - "WifiSignal": shared.get("WifiSignal", "N/A"), - "CellularSignal": shared.get("CellularSignal", "N/A"), - "TargetPairs": shared.get("target_pairs", []), - "WifiConnections": shared["WifiConnections"] + 'status_summary': shared.get('status_summary', 'Not Connected'), + 'uuid': shared.get('uuid', 'Unassigned'), + 'TailscaleIP': shared.get('TailscaleIP', 'N/A'), + 'DNSName': shared.get('DNSName', 'N/A'), + 'BatteryLevel': shared.get('battery_level', 'Unknown'), + 'WifiSignal': shared.get('WifiSignal', 'N/A'), + 'CellularSignal': shared.get('CellularSignal', 'N/A'), + 'TargetPairs': shared.get('target_pairs', []), + 'WifiConnections': shared['WifiConnections'], }) @app.route('/wifi', methods=['POST']) @@ -237,8 +232,7 @@ def index(): new_uuid = request.form.get('new_uuid', '') if new_ip and new_uuid: pairs = list(shared['target_pairs']) - pairs.append({'ip': new_ip, 'uuid': new_uuid, - 'status': "Not Connected"}) + pairs.append({'ip': new_ip, 'uuid': new_uuid, 'status': 'Not Connected'}) shared['target_pairs'] = pairs save_config(dict(shared)) elif action.startswith('remove_'): diff --git a/archive/protective_stop_remote/protective_stop_remote/button.py b/archive/protective_stop_remote/protective_stop_remote/button.py index f7d4f2c..b68ecab 100644 --- a/archive/protective_stop_remote/protective_stop_remote/button.py +++ b/archive/protective_stop_remote/protective_stop_remote/button.py @@ -1,31 +1,30 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - # Handles pstop button physical interaction -import RPi.GPIO as GPIO -import time import logging +import time + +import RPi.GPIO as GPIO # GPIO setup # 15 connected to 23 when button pressed # 14 connected to 23 when button released + def button_node(shared): logging.basicConfig(level=logging.DEBUG) OUTPUT_PIN = 23 INPUT_PINS = [14, 15] - - if "button_pressed" not in shared: - shared["button_pressed"] = None - + + if 'button_pressed' not in shared: + shared['button_pressed'] = None + try: GPIO.setmode(GPIO.BCM) # Use BCM pin numbering GPIO.setup(OUTPUT_PIN, GPIO.OUT) - - logging.info(f"Button Node starting at {time.time()}") + + logging.info(f'Button Node starting at {time.time()}') # The following code toggles output high and low, and checks for correct return - # If the value is not correct, one or both of the return lines is broken + # If the value is not correct, one or both of the return lines is broken while True: GPIO.output(OUTPUT_PIN, GPIO.HIGH) for pin in INPUT_PINS: @@ -55,23 +54,24 @@ def button_node(shared): new_state = None # Log only when the state changes - if new_state != shared["button_pressed"]: - shared["button_pressed"] = new_state + if new_state != shared['button_pressed']: + shared['button_pressed'] = new_state if new_state is True: - logging.info(f"Button pressed at {time.time()}") + logging.info(f'Button pressed at {time.time()}') elif new_state is False: - logging.info(f"Button released at {time.time()}") + logging.info(f'Button released at {time.time()}') else: - logging.info(f"Button state unknown (failure) at {time.time()}") + logging.info(f'Button state unknown (failure) at {time.time()}') time.sleep(0.05) # 20 Hz - + except KeyboardInterrupt: - logging.info(f"Button Node exiting at {time.time()}") - - finally: + logging.info(f'Button Node exiting at {time.time()}') + + finally: GPIO.cleanup() - + + # Running standalone if needed if __name__ == '__main__': button_node(shared={}) diff --git a/archive/protective_stop_remote/protective_stop_remote/main.py b/archive/protective_stop_remote/protective_stop_remote/main.py index 6e0de92..26b91d9 100644 --- a/archive/protective_stop_remote/protective_stop_remote/main.py +++ b/archive/protective_stop_remote/protective_stop_remote/main.py @@ -1,19 +1,15 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - # Automatically started on boot, combines data from other nodes -from multiprocessing import Process, Manager +from multiprocessing import Manager, Process -from power import power_node # Handles power button, UPS management, shutdown command -from ui import ui_node # Controls the LED ring, E paper display # Handles configuration via webpage + saving config to json file from app import run_app -# Main pstop node, communicates with machine node on robot -from pstop import pstop_node from button import button_node # Handles pstop button +from power import power_node # Handles power button, UPS management, shutdown command -import logging +# Main pstop node, communicates with machine node on robot +from pstop import pstop_node +from ui import ui_node # Controls the LED ring, E paper display if __name__ == '__main__': with Manager() as manager: @@ -22,7 +18,7 @@ # Create two processes, one for ROS node and one for UI node power_process = Process(target=power_node, args=(shared_data,)) - app_process = Process(target=run_app, args=(shared_data,)) + app_process = Process(target=run_app, args=(shared_data,)) ui_process = Process(target=ui_node, args=(shared_data,)) pstop_process = Process(target=pstop_node, args=(shared_data,)) button_process = Process(target=button_node, args=(shared_data,)) diff --git a/archive/protective_stop_remote/protective_stop_remote/power.py b/archive/protective_stop_remote/protective_stop_remote/power.py index 952a520..99f8e14 100644 --- a/archive/protective_stop_remote/protective_stop_remote/power.py +++ b/archive/protective_stop_remote/protective_stop_remote/power.py @@ -1,28 +1,25 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - - # Handles power button, UPS management, shutdown command +import logging +import subprocess import time + import smbus -import subprocess -import logging -import traceback # ========== I²C Setup (PiSugar) ========== I2C_BUS = 1 DEVICE_ADDR = 0x57 -REG_BUTTON = 0x02 # Register to read button state (LSB) & modify bits -REG_ACTION = 0x09 # Register to write an action/delay if needed +REG_BUTTON = 0x02 # Register to read button state (LSB) & modify bits +REG_ACTION = 0x09 # Register to write an action/delay if needed -PRESS_THRESHOLD = 0.2 # Number of seconds to hold button +PRESS_THRESHOLD = 0.2 # Number of seconds to hold button bus = smbus.SMBus(I2C_BUS) button_pressed_start = None + def shutdown_process(shared): - logging.info("==> Shut Down Sequence Initiated...") + logging.info('==> Shut Down Sequence Initiated...') shared['shutdown'] = True # Sets shutdown delay to 20 seconds @@ -33,7 +30,7 @@ def shutdown_process(shared): try: value = bus.read_byte_data(DEVICE_ADDR, REG_BUTTON) except OSError as e: - logging.warning(f"I2C Read Error: {e}. Retrying...") + logging.warning(f'I2C Read Error: {e}. Retrying...') continue # Clear the 5th bit (bit index 5) of 0x02 @@ -43,26 +40,25 @@ def shutdown_process(shared): try: bus.write_byte_data(DEVICE_ADDR, REG_BUTTON, new_value) verify_val = bus.read_byte_data(DEVICE_ADDR, REG_BUTTON) - logging.info(f"Verified 0x02 after write: 0x{verify_val:02X}") + logging.info(f'Verified 0x02 after write: 0x{verify_val:02X}') except OSError as e: - logging.warning(f"I2C Write/Verify Error (REG_BUTTON): {e}") - + logging.warning(f'I2C Write/Verify Error (REG_BUTTON): {e}') # ========== Issue System Shutdown ========== - logging.info("Issuing system shutdown command...") - subprocess.run(["sudo", "shutdown", "-h", "now"]) + logging.info('Issuing system shutdown command...') + subprocess.run(['sudo', 'shutdown', '-h', 'now']) -def power_node(shared): +def power_node(shared): logging.basicConfig(level=logging.DEBUG) # Optional: Make certain registers writable (per PiSugar docs, if needed) try: bus.write_byte_data(DEVICE_ADDR, 0x0B, 0x29) - logging.info("Setting 0x0B = 0x29 to enable writable registers.") + logging.info('Setting 0x0B = 0x29 to enable writable registers.') except OSError as e: - logging.warning(f"I2C Write Error (0x0B): {e}") - + logging.warning(f'I2C Write Error (0x0B): {e}') + shared['shutdown'] = False # ========== Main Loop ========== @@ -70,30 +66,31 @@ def power_node(shared): # ========== Battery Check ========== try: # Use shell=True so we can pipe directly in one command - battery_output = subprocess.check_output( - "echo 'get battery' | nc -q 0 127.0.0.1 8423", - shell=True - ).decode('utf-8').strip() + battery_output = ( + subprocess.check_output("echo 'get battery' | nc -q 0 127.0.0.1 8423", shell=True) + .decode('utf-8') + .strip() + ) # If battery_output is something like "37%" or "37", parse it # Remove any trailing '%' if present: battery_value = battery_output.replace('%', '').strip()[9:] - battery_percentage = round(float(battery_value),1) + battery_percentage = round(float(battery_value), 1) shared['battery_level'] = battery_percentage - #logging.warning(f"Battery level is ({battery_percentage}%).") + # logging.warning(f"Battery level is ({battery_percentage}%).") if battery_percentage < 4: - logging.warning(f"Battery level is below 4% ({battery_percentage}%).") + logging.warning(f'Battery level is below 4% ({battery_percentage}%).') shutdown_process(shared) except Exception as e: - logging.warning(f"Battery check failed: {e}") + logging.warning(f'Battery check failed: {e}') # ========== Button Handling ========== try: value = bus.read_byte_data(DEVICE_ADDR, REG_BUTTON) except OSError as e: - logging.warning(f"I2C Read Error: {e}. Retrying...") + logging.warning(f'I2C Read Error: {e}. Retrying...') time.sleep(0.1) continue @@ -108,7 +105,6 @@ def power_node(shared): else: # Check how long it's been held if (time.time() - button_pressed_start) >= PRESS_THRESHOLD: - shutdown_process(shared) # Reset press timer so we only do this once per press button_pressed_start = None @@ -117,7 +113,7 @@ def power_node(shared): button_pressed_start = None time.sleep(0.1) - - + + if __name__ == '__main__': power_node(shared={}) # Normal dict here for a simple test diff --git a/archive/protective_stop_remote/protective_stop_remote/pstop.py b/archive/protective_stop_remote/protective_stop_remote/pstop.py index 6ab5a6a..8616a49 100644 --- a/archive/protective_stop_remote/protective_stop_remote/pstop.py +++ b/archive/protective_stop_remote/protective_stop_remote/pstop.py @@ -1,10 +1,10 @@ -import time +import base64 import logging -import roslibpy import threading +import time import uuid -import base64 +import roslibpy # TODO: # - Handle connect/disconnect/connect more reliably @@ -46,8 +46,15 @@ def compute_crc16(data: bytes, poly: int = 0x1021, init: int = 0xFFFF) -> int: def build_pstop_message( - message_id, state_id, timestamp, sender_uuid, receiver_uuid, - counter, heartbeat_timeout, received_stamp, received_counter + message_id, + state_id, + timestamp, + sender_uuid, + receiver_uuid, + counter, + heartbeat_timeout, + received_stamp, + received_counter, ): """ Constructs a PStopMsg dictionary using the correct lifecycle state definitions. @@ -69,22 +76,22 @@ def build_pstop_message( # Lifecycle state definitions, from https://docs.ros2.org/foxy/api/lifecycle_msgs/msg/State.html lifecycle_states = { - 0: "UNKNOWN", - 1: "UNCONFIGURED", - 2: "INACTIVE", - 3: "ACTIVE", - 4: "FINALIZED", - 10: "CONFIGURING", - 11: "CLEANINGUP", - 12: "SHUTTINGDOWN", - 13: "ACTIVATING", - 14: "DEACTIVATING", - 15: "ERRORPROCESSING" + 0: 'UNKNOWN', + 1: 'UNCONFIGURED', + 2: 'INACTIVE', + 3: 'ACTIVE', + 4: 'FINALIZED', + 10: 'CONFIGURING', + 11: 'CLEANINGUP', + 12: 'SHUTTINGDOWN', + 13: 'ACTIVATING', + 14: 'DEACTIVATING', + 15: 'ERRORPROCESSING', } # Function to get state label dynamically def get_state_label(state_id): - return lifecycle_states.get(state_id, "UNKNOWN") + return lifecycle_states.get(state_id, 'UNKNOWN') return { 'message': message_id, @@ -95,9 +102,9 @@ def get_state_label(state_id): 'counter': counter, 'heartbeat_timeout': heartbeat_timeout, 'checksum_type': 'CRC-16', - 'checksum_value': f"{compute_crc16(str(counter).encode()):04X}", + 'checksum_value': f'{compute_crc16(str(counter).encode()):04X}', 'received_stamp': received_stamp, - 'received_counter': received_counter + 'received_counter': received_counter, } @@ -108,160 +115,161 @@ def start_client(ip, port, shared): while not client.is_connected and attempt_count < max_attempts: try: - logging.info( - f"Attempting to connect to {ip}:{port} (Attempt {attempt_count + 1}/{max_attempts})") + logging.info(f'Attempting to connect to {ip}:{port} (Attempt {attempt_count + 1}/{max_attempts})') client.run() client.connect() time.sleep(0.5) if client.is_connected: - logging.info(f"Connected to {ip}:{port}") - machine = next( - (m for m in shared["target_pairs"] if m.get("ip") == ip), None) + logging.info(f'Connected to {ip}:{port}') + machine = next((m for m in shared['target_pairs'] if m.get('ip') == ip), None) if machine: - machine['status'] = "Connected" + machine['status'] = 'Connected' else: - logging.error(f"Connection to {ip}:{port} failed") + logging.error(f'Connection to {ip}:{port} failed') client.close() except Exception as e: - logging.error(f"Connection to {ip}:{port} failed: {e}") + logging.error(f'Connection to {ip}:{port} failed: {e}') attempt_count += 1 - machine = next( - (m for m in shared["target_pairs"] if m.get("ip") == ip), None) + machine = next((m for m in shared['target_pairs'] if m.get('ip') == ip), None) if machine: - machine['status'] = f"Retrying ({attempt_count}/{max_attempts})" + machine['status'] = f'Retrying ({attempt_count}/{max_attempts})' if attempt_count < max_attempts: - logging.info("Retrying in 5 seconds...") + logging.info('Retrying in 5 seconds...') time.sleep(5) if not client.is_connected: - logging.error( - f"Failed to connect to {ip}:{port} after {max_attempts} attempts.") - machine = next( - (m for m in shared["target_pairs"] if m.get("ip") == ip), None) + logging.error(f'Failed to connect to {ip}:{port} after {max_attempts} attempts.') + machine = next((m for m in shared['target_pairs'] if m.get('ip') == ip), None) if machine: - machine['status'] = "Failed" + machine['status'] = 'Failed' return client def setup_client(client, ip, port, shared): - publisher = roslibpy.Topic( - client, '/protective_stop', 'pstop_msg/msg/PStopMsg') + publisher = roslibpy.Topic(client, '/protective_stop', 'pstop_msg/msg/PStopMsg') publisher.advertise() - subscriber = roslibpy.Topic( - client, '/protective_stop', 'pstop_msg/msg/PStopMsg') + subscriber = roslibpy.Topic(client, '/protective_stop', 'pstop_msg/msg/PStopMsg') bonding_status = {} # Track bonding state per machine UUID try: - self_uuid = uuid.UUID(shared.get("uuid")) + self_uuid = uuid.UUID(shared.get('uuid')) except ValueError: - logging.error(f"Invalid self UUID: {shared.get('uuid')}") + logging.error(f'Invalid self UUID: {shared.get("uuid")}') return def callback(message): - sender_uuid_raw = message.get("id", {}).get("uuid") - receiver_uuid_raw = message.get("receiver_uuid", {}).get("uuid") + sender_uuid_raw = message.get('id', {}).get('uuid') + receiver_uuid_raw = message.get('receiver_uuid', {}).get('uuid') sender_uuid = None receiver_uuid = None - logging.info(f"⚠️ Received message: {message}") + logging.info(f'⚠️ Received message: {message}') # ✅ Ensure sender UUID is extracted correctly if isinstance(sender_uuid_raw, list) and len(sender_uuid_raw) == 16: sender_uuid = uuid.UUID(bytes=bytes(sender_uuid_raw)) elif isinstance(sender_uuid_raw, str): try: - sender_uuid = uuid.UUID( - bytes=base64.b64decode(sender_uuid_raw)) + sender_uuid = uuid.UUID(bytes=base64.b64decode(sender_uuid_raw)) except Exception: - logging.warning( - f"⚠️ Failed to decode sender UUID from Base64: {sender_uuid_raw}") + logging.warning(f'⚠️ Failed to decode sender UUID from Base64: {sender_uuid_raw}') # ✅ Ensure receiver UUID is extracted correctly if isinstance(receiver_uuid_raw, list) and len(receiver_uuid_raw) == 16: receiver_uuid = uuid.UUID(bytes=bytes(receiver_uuid_raw)) elif isinstance(receiver_uuid_raw, str): try: - receiver_uuid = uuid.UUID( - bytes=base64.b64decode(receiver_uuid_raw)) + receiver_uuid = uuid.UUID(bytes=base64.b64decode(receiver_uuid_raw)) except Exception: - logging.warning( - f"⚠️ Failed to decode receiver UUID from Base64: {receiver_uuid_raw}") + logging.warning(f'⚠️ Failed to decode receiver UUID from Base64: {receiver_uuid_raw}') # ✅ Convert self UUID for consistent comparison try: - self_uuid = uuid.UUID(shared.get("uuid")) + self_uuid = uuid.UUID(shared.get('uuid')) except ValueError: - logging.error(f"❌ Invalid self UUID: {shared.get('uuid')}") + logging.error(f'❌ Invalid self UUID: {shared.get("uuid")}') return - logging.info( - f"Sender UUID: {sender_uuid}, Receiver UUID: {receiver_uuid}, Self UUID: {self_uuid}") + logging.info(f'Sender UUID: {sender_uuid}, Receiver UUID: {receiver_uuid}, Self UUID: {self_uuid}') if sender_uuid and sender_uuid == self_uuid: - logging.info( - f"❌ Ignoring self-generated message from UUID {self_uuid}") + logging.info(f'❌ Ignoring self-generated message from UUID {self_uuid}') return sender_str = str(sender_uuid) - state_id = message["state"]["id"] + state_id = message['state']['id'] if state_id == 13: # TRANSITION_STATE_ACTIVATING - logging.info(f"🔄 Bonding started with {sender_uuid}") - bonding_status[sender_str] = "ACTIVATING" + logging.info(f'🔄 Bonding started with {sender_uuid}') + bonding_status[sender_str] = 'ACTIVATING' response = build_pstop_message( - PSTOP_OK, 13, message["stamp"], message["id"], message["receiver_uuid"], - message["counter"], message["heartbeat_timeout"], - message["received_stamp"], message["received_counter"] + PSTOP_OK, + 13, + message['stamp'], + message['id'], + message['receiver_uuid'], + message['counter'], + message['heartbeat_timeout'], + message['received_stamp'], + message['received_counter'], ) publisher.publish(roslibpy.Message(response)) elif state_id == 3: # PRIMARY_STATE_ACTIVE - if bonding_status.get(sender_str) == "ACTIVATING": - logging.info(f"✅ Bonded successfully with {sender_uuid}") - bonding_status[sender_str] = "ACTIVE" + if bonding_status.get(sender_str) == 'ACTIVATING': + logging.info(f'✅ Bonded successfully with {sender_uuid}') + bonding_status[sender_str] = 'ACTIVE' elif state_id == 14: # TRANSITION_STATE_DEACTIVATING - logging.info(f"❌ Unbonding from {sender_uuid}") - bonding_status[sender_str] = "DEACTIVATING" + logging.info(f'❌ Unbonding from {sender_uuid}') + bonding_status[sender_str] = 'DEACTIVATING' response = build_pstop_message( - PSTOP_OK, 14, message["stamp"], message["id"], message["receiver_uuid"], - message["counter"], message["heartbeat_timeout"], - message["received_stamp"], message["received_counter"] + PSTOP_OK, + 14, + message['stamp'], + message['id'], + message['receiver_uuid'], + message['counter'], + message['heartbeat_timeout'], + message['received_stamp'], + message['received_counter'], ) publisher.publish(roslibpy.Message(response)) elif state_id == 2: # PRIMARY_STATE_INACTIVE - if bonding_status.get(sender_str) == "DEACTIVATING": - logging.info(f"✅ Successfully unbonded from {sender_uuid}") + if bonding_status.get(sender_str) == 'DEACTIVATING': + logging.info(f'✅ Successfully unbonded from {sender_uuid}') bonding_status.pop(sender_str, None) subscriber.subscribe(callback) - logging.info(f"Publisher and subscriber set up for {ip}:{port}") + logging.info(f'Publisher and subscriber set up for {ip}:{port}') def bonding_thread(): while True: - for target in shared.get("target_pairs", []): - target_uuid = target.get("uuid") - if target_uuid and target["ip"] == ip and target_uuid not in bonding_status: - logging.info(f"🔄 Initiating bonding with {target_uuid}") + for target in shared.get('target_pairs', []): + target_uuid = target.get('uuid') + if target_uuid and target['ip'] == ip and target_uuid not in bonding_status: + logging.info(f'🔄 Initiating bonding with {target_uuid}') bonding_msg = build_pstop_message( - PSTOP_OK, 13, # ✅ Uses TRANSITION_STATE_ACTIVATING from build_pstop_message() - {"sec": int(time.time()), "nanosec": int( - (time.time() % 1) * 1e9)}, - {"uuid": str(self_uuid)}, - {"uuid": target_uuid}, - 0, {"sec": 1, "nanosec": 500000000}, - {"sec": 0, "nanosec": 0}, 0 + PSTOP_OK, + 13, # ✅ Uses TRANSITION_STATE_ACTIVATING from build_pstop_message() + {'sec': int(time.time()), 'nanosec': int((time.time() % 1) * 1e9)}, + {'uuid': str(self_uuid)}, + {'uuid': target_uuid}, + 0, + {'sec': 1, 'nanosec': 500000000}, + {'sec': 0, 'nanosec': 0}, + 0, ) publisher.publish(roslibpy.Message(bonding_msg)) - bonding_status[target_uuid] = "ACTIVATING" + bonding_status[target_uuid] = 'ACTIVATING' time.sleep(1) @@ -273,55 +281,57 @@ def client_thread(ip, rosbridge_port, shared, connected_clients): client = start_client(ip, rosbridge_port, shared) publisher, subscriber = setup_client(client, ip, rosbridge_port, shared) connected_clients[ip] = { - "client": client, - "publisher": publisher, - "subscriber": subscriber, - "thread": threading.current_thread() + 'client': client, + 'publisher': publisher, + 'subscriber': subscriber, + 'thread': threading.current_thread(), } counter = 0 # ✅ Ensure receiver UUID is extracted correctly - receiver_uuid_str = next( - (m["uuid"] for m in shared["target_pairs"] if m["ip"] == ip), None) - receiver_uuid = uuid.UUID(receiver_uuid_str.strip( - )) if receiver_uuid_str else uuid.UUID(int=0) + receiver_uuid_str = next((m['uuid'] for m in shared['target_pairs'] if m['ip'] == ip), None) + receiver_uuid = uuid.UUID(receiver_uuid_str.strip()) if receiver_uuid_str else uuid.UUID(int=0) while ip in connected_clients: if not client.is_connected: - logging.info(f"Client {ip} disconnected, attempting reconnection.") + logging.info(f'Client {ip} disconnected, attempting reconnection.') client = start_client(ip, rosbridge_port, shared) - publisher, subscriber = setup_client( - client, ip, rosbridge_port, shared) + publisher, subscriber = setup_client(client, ip, rosbridge_port, shared) connected_clients[ip] = { - "client": client, - "publisher": publisher, - "subscriber": subscriber, - "thread": threading.current_thread() + 'client': client, + 'publisher': publisher, + 'subscriber': subscriber, + 'thread': threading.current_thread(), } # ✅ Convert self UUID properly - self_uuid = uuid.UUID(shared["uuid"].strip()) + self_uuid = uuid.UUID(shared['uuid'].strip()) # ✅ Convert both UUIDs to `uint8[16]` lists before sending - id = {"uuid": list(self_uuid.bytes)} - receiver_uuid = {"uuid": list(receiver_uuid.bytes)} + id = {'uuid': list(self_uuid.bytes)} + receiver_uuid = {'uuid': list(receiver_uuid.bytes)} # ✅ Build and send a protective stop message - timestamp = {'sec': int(time.time()), 'nanosec': int( - (time.time() % 1) * 1e9)} + timestamp = {'sec': int(time.time()), 'nanosec': int((time.time() % 1) * 1e9)} state = {'id': 1, 'label': 'ACTIVE'} heartbeat_timeout = {'sec': 1, 'nanosec': 500000000} - received_stamp = { - 'sec': timestamp['sec'], 'nanosec': timestamp['nanosec']} + received_stamp = {'sec': timestamp['sec'], 'nanosec': timestamp['nanosec']} received_counter = counter button = PSTOP_STOP - if shared.get("button_pressed") is False: + if shared.get('button_pressed') is False: button = PSTOP_OK # ✅ Only set to OK if the button is NOT pressed message = build_pstop_message( - button, state["id"], timestamp, id, receiver_uuid, counter, - heartbeat_timeout, received_stamp, received_counter + button, + state['id'], + timestamp, + id, + receiver_uuid, + counter, + heartbeat_timeout, + received_stamp, + received_counter, ) publisher.publish(roslibpy.Message(message)) counter += 1 @@ -331,62 +341,56 @@ def client_thread(ip, rosbridge_port, shared, connected_clients): def pstop_node(shared): logging.basicConfig(level=logging.DEBUG) - logging.info(f"PStop Node started at {time.time()}") + logging.info(f'PStop Node started at {time.time()}') - while "target_pairs" not in shared: + while 'target_pairs' not in shared: time.sleep(0.1) connected_clients = {} try: while True: - current_machines = {machine['ip'] for machine in shared.get( - "target_pairs", {}) if 'ip' in machine} + current_machines = {machine['ip'] for machine in shared.get('target_pairs', {}) if 'ip' in machine} current_ips = set(connected_clients.keys()) ips_to_add = current_machines - current_ips ips_to_remove = current_ips - current_machines # Add new connections for ip in ips_to_add: - logging.info(f"Starting client thread for {ip}") - thread = threading.Thread(target=client_thread, args=( - ip, rosbridge_port, shared, connected_clients)) + logging.info(f'Starting client thread for {ip}') + thread = threading.Thread(target=client_thread, args=(ip, rosbridge_port, shared, connected_clients)) thread.daemon = True thread.start() # Remove old connections for ip in ips_to_remove: - logging.info(f"Stopping client thread for {ip}") + logging.info(f'Stopping client thread for {ip}') connection = connected_clients.pop(ip, None) if connection: - connection["publisher"].unadvertise() - connection["subscriber"].unsubscribe() - connection["client"].close() + connection['publisher'].unadvertise() + connection['subscriber'].unsubscribe() + connection['client'].close() time.sleep(1) except KeyboardInterrupt: - logging.info("PStop Node exiting.") + logging.info('PStop Node exiting.') finally: for ip, connection in connected_clients.items(): - logging.info(f"Cleaning up connection for {ip}") - connection["publisher"].unadvertise() - connection["subscriber"].unsubscribe() - connection["client"].terminate() + logging.info(f'Cleaning up connection for {ip}') + connection['publisher'].unadvertise() + connection['subscriber'].unsubscribe() + connection['client'].terminate() # Running standalone for testing, with some default values if __name__ == '__main__': shared = { - "target_pairs": [ - { - "ip": "100.125.44.33", - "uuid": "00000000-0000-0000-0000-000000000000", - "status": "Not Connected" - } + 'target_pairs': [ + {'ip': '100.125.44.33', 'uuid': '00000000-0000-0000-0000-000000000000', 'status': 'Not Connected'} ], - "button_pressed": False, - "uuid": "00000000-0000-0000-0000-000000000001", + 'button_pressed': False, + 'uuid': '00000000-0000-0000-0000-000000000001', } pstop_node(shared) diff --git a/archive/protective_stop_remote/protective_stop_remote/templates/index.html b/archive/protective_stop_remote/protective_stop_remote/templates/index.html index a51812a..6ff341b 100644 --- a/archive/protective_stop_remote/protective_stop_remote/templates/index.html +++ b/archive/protective_stop_remote/protective_stop_remote/templates/index.html @@ -9,7 +9,7 @@ .then(response => response.json()) .then(data => { document.getElementById('status_summary').textContent = data.status_summary; - + // Update Tailscale information document.getElementById('tailscale-ip').textContent = data.TailscaleIP; document.getElementById('dns-name').textContent = data.DNSName; @@ -61,7 +61,7 @@

- @@ -84,19 +84,19 @@
Polymath Robotics Logo
- +

Status:

- +

Tailscale IP:

DNS Name:

UUID:

- +

Battery Level:

Wi-Fi Signal Strength:

Cellular Signal Strength:

- +

Target IP/UUID Pairs

@@ -147,4 +147,3 @@

Add a New Wi-Fi Connection

- diff --git a/archive/protective_stop_remote/protective_stop_remote/ui.py b/archive/protective_stop_remote/protective_stop_remote/ui.py index 682ea93..154077f 100644 --- a/archive/protective_stop_remote/protective_stop_remote/ui.py +++ b/archive/protective_stop_remote/protective_stop_remote/ui.py @@ -1,6 +1,3 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - # Controls the LED ring, E paper display # Due to control of LED ring, must be run as root on raspi @@ -19,44 +16,45 @@ # ESTOP = Spinning Red # Lost Connection = Flashing Yellow -from waveshare_epd import epd2in13_V4 -from PIL import Image, ImageDraw, ImageFont - +import logging +import math import time + import board import neopixel -import logging - +from PIL import Image, ImageDraw, ImageFont +from waveshare_epd import epd2in13_V4 pixel_pin = board.D18 # The number of NeoPixels num_pixels = 16 ORDER = neopixel.GRB -pixels = neopixel.NeoPixel( - pixel_pin, num_pixels, brightness=50, auto_write=False, pixel_order=ORDER -) +pixels = neopixel.NeoPixel(pixel_pin, num_pixels, brightness=50, auto_write=False, pixel_order=ORDER) # NeoPixel ring configuration PIXEL_PIN = board.D18 # GPIO pin connected to the NeoPixels -NUM_PIXELS = 16 # Number of pixels in the ring -BRIGHTNESS = 1 # Brightness of the LEDs (0.0 to 1.0) -GAMMA = 1.8 # Gamma correction value for human eyesight -FADE_FACTOR = 0.6 # Base fading factor for trail (stronger fade) -TRAIL_LENGTH = 7 # Number of pixels in the fading trail -FRAME_DELAY = 0.05 # Delay between frames in seconds +NUM_PIXELS = 16 # Number of pixels in the ring +BRIGHTNESS = 1 # Brightness of the LEDs (0.0 to 1.0) +GAMMA = 1.8 # Gamma correction value for human eyesight +FADE_FACTOR = 0.6 # Base fading factor for trail (stronger fade) +TRAIL_LENGTH = 7 # Number of pixels in the fading trail +FRAME_DELAY = 0.05 # Delay between frames in seconds font24 = ImageFont.truetype('/home/administrator/static/Font.ttc', 12) logo = Image.open('/home/administrator/static/images/polymath_logo_small.png') + def gamma_correct(value, gamma): """Apply gamma correction to a value.""" return int((value / 255) ** gamma * 255) + def fade_pixel(color, factor): """Fade a color by a given factor with gamma correction.""" return tuple(gamma_correct(int(c * factor), GAMMA) for c in color) - + + def fading_trail(): """Create a fading trail effect with two red segments chasing each other.""" segment1 = 0 @@ -84,11 +82,13 @@ def fading_trail(): # Wait a bit before the next frame time.sleep(FRAME_DELAY) + def solid_color(color): """Set all pixels to a solid color.""" pixels.fill(color) pixels.show() + def pulsing_color(color, duration=2): """Pulse the LEDs with the given color over a specified duration.""" steps = 50 @@ -99,6 +99,7 @@ def pulsing_color(color, duration=2): pixels.show() time.sleep(duration / steps) + def flashing_color(color, interval=0.5): """Flash the LEDs on and off with the given color at a specified interval.""" for _ in range(int(5 / interval)): @@ -109,25 +110,24 @@ def flashing_color(color, interval=0.5): pixels.show() time.sleep(interval) -def ui_node(shared): +def ui_node(shared): # ========== E-Paper Setup ========== try: # Instantiate the display epd = epd2in13_V4.EPD() - logging.info("Initializing e-paper...") + logging.info('Initializing e-paper...') epd.init_fast() - #epd.init() - #epd.Clear(0xFF) - + # epd.init() + # epd.Clear(0xFF) + # Display startup logo image1 = Image.new('1', (epd.height, epd.width), 255) # 255: clear the frame - image1.paste(logo, (2,2)) + image1.paste(logo, (2, 2)) epd.display(epd.getbuffer(image1)) - except IOError as e: - logging.error(f"E-Paper Initialization Error: {e}") + logging.error(f'E-Paper Initialization Error: {e}') epd = None # If e-paper fails, we skip display updates # Update function for the E-Paper display layout @@ -155,22 +155,21 @@ def update_display(shared, epd, font): high_level_status = shared.get('status_summary', 'Unknown') # Draw details - draw.text((start_x+34, start_y-2), f"{tailscale_dns}", font=font, fill=0) - draw.text((start_x+180, start_y+ 1 * line_spacing), f"Bat: {battery_level}%", font=font, fill=0) - draw.text((start_x+34, start_y + 1 * line_spacing), f"WiFi: {wifi_signal}%", font=font, fill=0) - draw.text((start_x+110, start_y + 1 * line_spacing), f"Cell: {cell_signal}%", font=font, fill=0) - draw.text((start_x, start_y + 2 * line_spacing), f"Status: {high_level_status}", font=font, fill=0) - draw.text((start_x, start_y + 3 * line_spacing), f"UUID: {uuid}", font=font, fill=0) - draw.text((start_x, start_y + 4 * line_spacing), f"Target UUID: {target_uuid}", font=font, fill=0) - draw.text((start_x, start_y + 5 * line_spacing), f"Target IP: {target_ip}", font=font, fill=0) - + draw.text((start_x + 34, start_y - 2), f'{tailscale_dns}', font=font, fill=0) + draw.text((start_x + 180, start_y + 1 * line_spacing), f'Bat: {battery_level}%', font=font, fill=0) + draw.text((start_x + 34, start_y + 1 * line_spacing), f'WiFi: {wifi_signal}%', font=font, fill=0) + draw.text((start_x + 110, start_y + 1 * line_spacing), f'Cell: {cell_signal}%', font=font, fill=0) + draw.text((start_x, start_y + 2 * line_spacing), f'Status: {high_level_status}', font=font, fill=0) + draw.text((start_x, start_y + 3 * line_spacing), f'UUID: {uuid}', font=font, fill=0) + draw.text((start_x, start_y + 4 * line_spacing), f'Target UUID: {target_uuid}', font=font, fill=0) + draw.text((start_x, start_y + 5 * line_spacing), f'Target IP: {target_ip}', font=font, fill=0) # Update display epd.displayPartial(epd.getbuffer(image)) - #epd.display_fast(epd.getbuffer(image)) + # epd.display_fast(epd.getbuffer(image)) except IOError as e: - logging.error(f"Display Update Error: {e}") + logging.error(f'Display Update Error: {e}') def display_shutdown_message(): if epd: @@ -184,13 +183,13 @@ def display_shutdown_message(): uuid = shared.get('uuid', 'N/A').split('-')[-1] tailscale_dns = shared.get('DNSName', 'N/A')[:-1] battery_level = shared.get('battery_level', 'Unknown') - - text = "PSTOP Powered Off" + + text = 'PSTOP Powered Off' text_x, text_y = 10, 40 # Adjust positioning as needed draw.text((text_x, text_y), text, font=font24, fill=0) - draw.text((text_x, text_y+ 1*20), f"{tailscale_dns}", font=font24, fill=0) - draw.text((text_x, text_y + 2 * 20), f"UUID: {uuid}", font=font24, fill=0) - draw.text((text_x, text_y + 3 * 20), f"Battery: {battery_level}%", font=font24, fill=0) + draw.text((text_x, text_y + 1 * 20), f'{tailscale_dns}', font=font24, fill=0) + draw.text((text_x, text_y + 2 * 20), f'UUID: {uuid}', font=font24, fill=0) + draw.text((text_x, text_y + 3 * 20), f'Battery: {battery_level}%', font=font24, fill=0) # Display in partial update (for speed) or full update epd.displayPartBaseImage(epd.getbuffer(image)) @@ -200,7 +199,7 @@ def display_shutdown_message(): epd.sleep() except IOError as e: - logging.error(f"E-Paper Display Error: {e}") + logging.error(f'E-Paper Display Error: {e}') try: while True: @@ -210,7 +209,7 @@ def display_shutdown_message(): pixels.show() display_shutdown_message() break - + if epd: # Update e-paper display update_display(shared, epd, font24) @@ -218,19 +217,19 @@ def display_shutdown_message(): if 'status_summary' in shared: status = shared['status_summary'] - if status == "Not Connected": + if status == 'Not Connected': solid_color((0, 0, 255)) # Solid Blue - elif status == "Connecting": + elif status == 'Connecting': pulsing_color((0, 0, 255)) # Pulsing Blue - elif status == "OK": + elif status == 'OK': solid_color((0, 255, 0)) # Solid Green - elif status == "ESTOP": + elif status == 'ESTOP': fading_trail() # Spinning Red (already implemented) - elif status == "Lost Connection": + elif status == 'Lost Connection': flashing_color((255, 255, 0)) # Flashing Yellow else: @@ -239,11 +238,11 @@ def display_shutdown_message(): time.sleep(0.1) # Adjust polling interval as needed except KeyboardInterrupt: - print("[UI Node] Shutting down") + print('[UI Node] Shutting down') pixels.fill((0, 0, 0)) pixels.show() + # Running standalone if needed if __name__ == '__main__': - ui_node(shared={'status_summary': "Not Connected"}) - + ui_node(shared={'status_summary': 'Not Connected'}) diff --git a/archive/protective_stop_remote/rosdep.yaml b/archive/protective_stop_remote/rosdep.yaml index d7cff43..60e31fc 100644 --- a/archive/protective_stop_remote/rosdep.yaml +++ b/archive/protective_stop_remote/rosdep.yaml @@ -1,10 +1,11 @@ +--- python3-roslibpy: - ubuntu: - pip: + ubuntu: + pip: - roslibpy - debian: - pip: + debian: + pip: - roslibpy - osx: - pip: + osx: + pip: - roslibpy diff --git a/archive/protective_stop_remote/setup.py b/archive/protective_stop_remote/setup.py index 9cfea4b..0c936d3 100644 --- a/archive/protective_stop_remote/setup.py +++ b/archive/protective_stop_remote/setup.py @@ -1,33 +1,25 @@ -from setuptools import setup, find_packages import os from glob import glob -package_name = "protective_stop_remote" +from setuptools import find_packages, setup + +package_name = 'protective_stop_remote' setup( name=package_name, - version="0.0.0", - packages=find_packages( - include=["protective_stop_remote", "protective_stop_remote.*"]), - install_requires=[ - "RPi.GPIO", - "flask", - "roslibpy", - "smbus" - ], + version='0.0.0', + packages=find_packages(include=['protective_stop_remote', 'protective_stop_remote.*']), + install_requires=['RPi.GPIO', 'flask', 'roslibpy', 'smbus'], entry_points={ - "console_scripts": [ - "protective_stop_remote = protective_stop_remote.main:main", + 'console_scripts': [ + 'protective_stop_remote = protective_stop_remote.main:main', ], }, data_files=[ ('share/' + package_name, ['package.xml']), - (os.path.join("share/ament_index/resource_index/packages"), - ['resource/' + package_name]), - + (os.path.join('share/ament_index/resource_index/packages'), ['resource/' + package_name]), ('share/' + package_name + '/tests', glob('tests/*.py')), ('share/' + package_name + '/tests', glob('tests/pytest.ini')), - ], zip_safe=True, ) diff --git a/archive/protective_stop_remote/tests/test_app.py b/archive/protective_stop_remote/tests/test_app.py index 393adb8..f57ccff 100644 --- a/archive/protective_stop_remote/tests/test_app.py +++ b/archive/protective_stop_remote/tests/test_app.py @@ -9,6 +9,6 @@ def client(): def test_data_endpoint(client): - response = client.get("/data") + response = client.get('/data') assert response.status_code == 200 - assert response.json["status_summary"] == "Not Connected" + assert response.json['status_summary'] == 'Not Connected' diff --git a/protective_stop_node/launch/protective_stop_node.launch.yaml b/protective_stop_node/launch/protective_stop_node.launch.yaml index e49c011..1488f8b 100644 --- a/protective_stop_node/launch/protective_stop_node.launch.yaml +++ b/protective_stop_node/launch/protective_stop_node.launch.yaml @@ -3,24 +3,24 @@ launch: - arg: name: machine_uuid - default: "xxxx" - description: "Machine UUID" + default: xxxx + description: Machine UUID - arg: name: heartbeat_timeout - default: "0.1" - description: "Heartbeat timeout" + default: '0.1' + description: Heartbeat timeout - arg: name: deactivation_timeout - default: "3.0" - description: "Deactivation timeout" + default: '3.0' + description: Deactivation timeout - arg: name: max_pstop_count - default: "3" - description: "Maximum protective stop count" + default: '3' + description: Maximum protective stop count - arg: name: is_user_monitored - default: "true" - description: "Enable or disable user monitoring mode" + default: 'true' + description: Enable or disable user monitoring mode - node: pkg: protective_stop_node exec: protective_stop_node @@ -48,6 +48,6 @@ launch: - name: bond_timeout value: 0.0 - name: node_names - value: ["protective_stop_node"] + value: [protective_stop_node] - name: use_sim_time value: true diff --git a/protective_stop_node/protective_stop_node/models.py b/protective_stop_node/protective_stop_node/models.py index 9d3ffbd..3f002a5 100644 --- a/protective_stop_node/protective_stop_node/models.py +++ b/protective_stop_node/protective_stop_node/models.py @@ -1,33 +1,29 @@ -from pydantic import BaseModel, Field, validator -from pydantic import validator, root_validator -from pydantic.json import pydantic_encoder +from enum import Enum +from typing import Optional + +from builtin_interfaces.msg import Time as TimeMsg +from pydantic import BaseModel, Field, root_validator, validator from rclpy.time import Time -from typing import Optional, List, TypeVar, Generic, Dict from protective_stop_msg.msg import ( - ProtectiveStop, - ProtectiveStopStatus, - ProtectiveStopParams, ProtectiveStopDebugRemote, + ProtectiveStopStatus, ) -from enum import Enum -from builtin_interfaces.msg import Time as TimeMsg - class Ros2Time(BaseModel): - sec: int = Field(..., ge=0, description="Seconds since epoch") - nanosec: int = Field(..., ge=0, lt=1_000_000_000, description="Nanoseconds component") + sec: int = Field(..., ge=0, description='Seconds since epoch') + nanosec: int = Field(..., ge=0, lt=1_000_000_000, description='Nanoseconds component') @classmethod - def from_ros_time(cls, ros_time: Time) -> "Ros2Time": + def from_ros_time(cls, ros_time: Time) -> 'Ros2Time': """ Convert an `rclpy.time.Time` object to `Ros2Time`. """ return cls(sec=ros_time.seconds_nanoseconds()[0], nanosec=ros_time.seconds_nanoseconds()[1]) @classmethod - def from_time_msg(cls, time_msg: TimeMsg) -> "Ros2Time": + def from_time_msg(cls, time_msg: TimeMsg) -> 'Ros2Time': """ Convert a `builtin_interfaces.msg.Time` message to `Ros2Time`. """ @@ -54,13 +50,11 @@ class PStopRemoteStatusEnum(Enum): class ConnectionStatus(BaseModel): status: PStopRemoteStatusEnum = Field(PStopRemoteStatusEnum.ACTIVE) - message: str = Field("") + message: str = Field('') - @validator("status") + @validator('status') def check_status_type(cls, value): - assert isinstance(value, PStopRemoteStatusEnum), ( - "status must be of type PStopRemoteStatusEnum" - ) + assert isinstance(value, PStopRemoteStatusEnum), 'status must be of type PStopRemoteStatusEnum' return value def to_ros_message(self) -> ProtectiveStopStatus: @@ -84,10 +78,10 @@ def convert_ros_time(cls, values): Converts `rclpy.time.Time` or `builtin_interfaces.msg.Time` into `Ros2Time` before validation. """ - if isinstance(values.get("ros_time"), Time): - values["ros_time"] = Ros2Time.from_ros_time(values["ros_time"]) - elif isinstance(values.get("ros_time"), TimeMsg): - values["ros_time"] = Ros2Time.from_time_msg(values["ros_time"]) + if isinstance(values.get('ros_time'), Time): + values['ros_time'] = Ros2Time.from_ros_time(values['ros_time']) + elif isinstance(values.get('ros_time'), TimeMsg): + values['ros_time'] = Ros2Time.from_time_msg(values['ros_time']) return values # Convert into ros message @@ -98,7 +92,5 @@ def to_ros_message(self, uuid: str) -> ProtectiveStopDebugRemote: connection_status=self.connection_status.to_ros_message(), counter=self.counter, init_timestamp=self.init_timestamp.to_time_msg(), - receive_timestamp=self.receive_timestamp.to_time_msg() - if self.receive_timestamp - else None, + receive_timestamp=self.receive_timestamp.to_time_msg() if self.receive_timestamp else None, ) diff --git a/protective_stop_node/protective_stop_node/protective_stop_node.py b/protective_stop_node/protective_stop_node/protective_stop_node.py index 91e7c3d..fd26ed1 100644 --- a/protective_stop_node/protective_stop_node/protective_stop_node.py +++ b/protective_stop_node/protective_stop_node/protective_stop_node.py @@ -1,77 +1,68 @@ -#!/usr/bin/env python3 +import threading +import traceback +from functools import partial + import rclpy +from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult +from rclpy.duration import Duration +from rclpy.lifecycle import LifecycleNode, State, TransitionCallbackReturn +from termcolor import colored + from protective_stop_msg.msg import ( ProtectiveStop, - ProtectiveStopParams, - ProtectiveStopHeartbeat, ProtectiveStopDebug, + ProtectiveStopHeartbeat, + ProtectiveStopParams, ) from protective_stop_msg.srv import ( ProtectiveStop as ProtectiveStopSrv, ) -from termcolor import colored -import threading - from protective_stop_node.models import ( + ConnectionStatus, PStopModel, PStopRemoteStatusEnum, Ros2Time, - ConnectionStatus, ) -from rclpy.duration import Duration -import traceback -from functools import partial - - -from rclpy.lifecycle import State, TransitionCallbackReturn, LifecycleNode - -from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult - - -PROTECTIVE_STOP_TOPIC = "/protective_stop" -PROTECTIVE_STOP_HB_TOPIC = "/pstop_hb" -PROTECTIVE_STOP_DEBUG_TOPIC = "/protective_stop/debug" +PROTECTIVE_STOP_TOPIC = '/protective_stop' +PROTECTIVE_STOP_HB_TOPIC = '/pstop_hb' +PROTECTIVE_STOP_DEBUG_TOPIC = '/protective_stop/debug' class ProtectiveStopNode(LifecycleNode): def __init__(self, node_name): super().__init__(node_name) - self.get_logger().debug(f"Initializing {node_name}") + self.get_logger().debug(f'Initializing {node_name}') # Declaring ROS2 params + self.declare_parameter('machine_uuid', '', ParameterDescriptor(description='Machine uuid of robot')) self.declare_parameter( - "machine_uuid", "", ParameterDescriptor(description="Machine uuid of robot") - ) - self.declare_parameter( - "heartbeat_timeout", + 'heartbeat_timeout', 1.0, - ParameterDescriptor( - description="timeout float value in seconds before pstop is triggered" - ), + ParameterDescriptor(description='timeout float value in seconds before pstop is triggered'), ) self.declare_parameter( - "deactivation_timeout", + 'deactivation_timeout', 5.0, ParameterDescriptor( - description="Wait time in seconds before deactivating a remote pstop and deleting it from internal register" + description='Wait time in seconds before deactivating a remote pstop and deleting it from internal register' ), ) self.declare_parameter( - "max_pstop_count", + 'max_pstop_count', 10, ParameterDescriptor( - description="Maximum number of pstops that can be registered to the protective stop node" + description='Maximum number of pstops that can be registered to the protective stop node' ), ) self.declare_parameter( - "is_user_monitored", + 'is_user_monitored', True, ParameterDescriptor( - description="If True, the node will monitor connected pstops and enforce their state. If False, it will not enforce that a pstop remote is connected to the node in order for it to send healthy heartbeats." + description='If True, the node will monitor connected pstops and enforce their state. If False, it will not enforce that a pstop remote is connected to the node in order for it to send healthy heartbeats.' ), ) @@ -90,40 +81,30 @@ def __init__(self, node_name): self.param_event_handler = None self.is_user_monitored = None - self.activate_service = self.create_service( ProtectiveStopSrv, - f"{self.get_name()}/activate", + f'{self.get_name()}/activate', partial(self.state_transition_callback, True), ) self.deactivate_service = self.create_service( ProtectiveStopSrv, - f"{self.get_name()}/deactivate", + f'{self.get_name()}/deactivate', partial(self.state_transition_callback, False), ) self.add_on_set_parameters_callback(self.parameters_callback) - def on_configure(self, state: State) -> TransitionCallbackReturn: self.get_logger().info(f"Configuring node '{self.get_name()}' in state '{state.label}'.") - self.heartbeat_timeout = ( - self.get_parameter("heartbeat_timeout").get_parameter_value().double_value - ) - self.deactivation_timeout = ( - self.get_parameter("deactivation_timeout").get_parameter_value().double_value - ) + self.heartbeat_timeout = self.get_parameter('heartbeat_timeout').get_parameter_value().double_value + self.deactivation_timeout = self.get_parameter('deactivation_timeout').get_parameter_value().double_value - self.machine_uuid = self.get_parameter("machine_uuid").get_parameter_value().string_value + self.machine_uuid = self.get_parameter('machine_uuid').get_parameter_value().string_value - self.max_pstop_count = ( - self.get_parameter("max_pstop_count").get_parameter_value().integer_value - ) + self.max_pstop_count = self.get_parameter('max_pstop_count').get_parameter_value().integer_value - self.is_user_monitored = ( - self.get_parameter("is_user_monitored").get_parameter_value().bool_value - ) + self.is_user_monitored = self.get_parameter('is_user_monitored').get_parameter_value().bool_value self.set_is_user_monitored(self.is_user_monitored) self.get_logger().info( @@ -136,7 +117,7 @@ def on_configure(self, state: State) -> TransitionCallbackReturn: - deactivation timeout: {self.deactivation_timeout} - user monitor mode: {self.is_user_monitored} """, - "cyan", + 'cyan', ) ) @@ -145,25 +126,17 @@ def on_configure(self, state: State) -> TransitionCallbackReturn: def on_activate(self, state: State) -> TransitionCallbackReturn: self.get_logger().info(f"Activating node '{self.get_name()}' in state '{state.label}'.") - self.subscriber_pstop = self.create_subscription( - ProtectiveStop, PROTECTIVE_STOP_TOPIC, self.pstop_callback, 1 - ) + self.subscriber_pstop = self.create_subscription(ProtectiveStop, PROTECTIVE_STOP_TOPIC, self.pstop_callback, 1) self.publisher_pstop = self.create_publisher(ProtectiveStop, PROTECTIVE_STOP_TOPIC, 1) - self.publisher_pstop_debug = self.create_publisher( - ProtectiveStopDebug, PROTECTIVE_STOP_DEBUG_TOPIC, 1 - ) - self.publisher_hb = self.create_publisher( - ProtectiveStopHeartbeat, PROTECTIVE_STOP_HB_TOPIC, 1 - ) + self.publisher_pstop_debug = self.create_publisher(ProtectiveStopDebug, PROTECTIVE_STOP_DEBUG_TOPIC, 1) + self.publisher_hb = self.create_publisher(ProtectiveStopHeartbeat, PROTECTIVE_STOP_HB_TOPIC, 1) self.hb_timer = self.create_timer(self.heartbeat_timeout, self.process_heartbeat) self.debug_timer = self.create_timer(1.0, self._publish_debug_msg) return TransitionCallbackReturn.SUCCESS def on_deactivate(self, state: State) -> TransitionCallbackReturn: - self.get_logger().info( - colored(f"Deactivating node '{self.get_name()}' in state '{state.label}'.", "cyan") - ) + self.get_logger().info(colored(f"Deactivating node '{self.get_name()}' in state '{state.label}'.", 'cyan')) if self.hb_timer is not None: self.hb_timer.cancel() if self.debug_timer is not None: @@ -180,7 +153,7 @@ def on_deactivate(self, state: State) -> TransitionCallbackReturn: if self.publisher_pstop_debug: self.destroy_publisher(self.publisher_pstop_debug) self - self.get_logger().info("Node deactivated successfully.") + self.get_logger().info('Node deactivated successfully.') return TransitionCallbackReturn.SUCCESS def on_shutdown(self, state: State) -> TransitionCallbackReturn: @@ -192,12 +165,9 @@ def parameters_callback(self, params): Callback for parameter updates """ for param in params: - if param.name == "is_user_monitored": + if param.name == 'is_user_monitored': self.set_is_user_monitored(param.value) - return SetParametersResult( - successful=True, - reason="Parameter update processed successfully." - ) + return SetParametersResult(successful=True, reason='Parameter update processed successfully.') def set_is_user_monitored(self, is_user_monitored: bool): """ @@ -211,11 +181,15 @@ def set_is_user_monitored(self, is_user_monitored: bool): """ self.is_user_monitored = is_user_monitored - warning_str = 'User Monitor Mode DISABLED. If you lose connection to your protective stop on machine, it will continue to produce a heartbeat.' if not self.is_user_monitored else 'User Monitor Mode ENABLED.' + warning_str = ( + 'User Monitor Mode DISABLED. If you lose connection to your protective stop on machine, it will continue to produce a heartbeat.' + if not self.is_user_monitored + else 'User Monitor Mode ENABLED.' + ) self.get_logger().warn( colored( - f"Received request to set monitoring mode. Current state: {warning_str}", - "yellow", + f'Received request to set monitoring mode. Current state: {warning_str}', + 'yellow', ) ) @@ -225,11 +199,11 @@ def state_transition_callback(self, activate: bool, request, response): """ uuid = request.sender_uuid recv_uuid = request.recv_uuid - state = "ACTIVE" if activate else "DEACTIVATED" + state = 'ACTIVE' if activate else 'DEACTIVATED' self.get_logger().info( colored( f"Received state transition request from {uuid} to {recv_uuid} to state '{state}'", - "cyan", + 'cyan', ) ) @@ -240,16 +214,16 @@ def state_transition_callback(self, activate: bool, request, response): if uuid == self.machine_uuid: response.success = False - response.message = "Cannot send a state transition to self" + response.message = 'Cannot send a state transition to self' return response elif recv_uuid != self.machine_uuid: response.success = False - response.message = "Unknown receiver uuid" + response.message = 'Unknown receiver uuid' return response # TODO(troy): Update version here to constant elif request.version != ProtectiveStop.VERSION: response.success = False - response.message = "Invalid version" + response.message = 'Invalid version' return response if activate: @@ -260,8 +234,8 @@ def state_transition_callback(self, activate: bool, request, response): ) self.get_logger().warn( colored( - f"Max pstop count of {self.max_pstop_count} reached. Pruning oldest pstop: {oldest_pstop[0][0]}", - "yellow", + f'Max pstop count of {self.max_pstop_count} reached. Pruning oldest pstop: {oldest_pstop[0][0]}', + 'yellow', ), throttle_duration_sec=1.0, ) @@ -276,19 +250,17 @@ def state_transition_callback(self, activate: bool, request, response): ): self.get_logger().warn( colored( - f"Received activation request from {uuid} while already activated", - "yellow", + f'Received activation request from {uuid} while already activated', + 'yellow', ), throttle_duration_sec=1.0, ) response.success = False - response.message = f"Already activated. Current state of remote is {self.connected_remote_pstop_state[uuid].connection_status.status.name}" + response.message = f'Already activated. Current state of remote is {self.connected_remote_pstop_state[uuid].connection_status.status.name}' return response pstop_state = PStopModel( - connection_status=ConnectionStatus( - status=PStopRemoteStatusEnum.ACTIVE, message="Activated by remote" - ), + connection_status=ConnectionStatus(status=PStopRemoteStatusEnum.ACTIVE, message='Activated by remote'), init_timestamp=Ros2Time.from_ros_time(self.get_clock().now()), receive_timestamp=Ros2Time.from_ros_time(self.get_clock().now()), remote_timestamp=None, @@ -296,20 +268,20 @@ def state_transition_callback(self, activate: bool, request, response): self.connected_remote_pstop_state[uuid] = pstop_state response.success = True - response.message = "Successfully activated" + response.message = 'Successfully activated' return response else: if uuid in self.connected_remote_pstop_state: self.connected_remote_pstop_state[uuid].connection_status = ConnectionStatus( - status=PStopRemoteStatusEnum.DEACTIVATED, message="Deactivated by remote" + status=PStopRemoteStatusEnum.DEACTIVATED, message='Deactivated by remote' ) else: response.success = False - response.message = "Deactivation failed. UUID not in connected list" + response.message = 'Deactivation failed. UUID not in connected list' return response response.success = True - response.message = "Successfully deactivated" + response.message = 'Successfully deactivated' return response def _get_status_string(self, connection_status: ConnectionStatus): @@ -326,46 +298,33 @@ def pstop_callback(self, msg): and (msg.version == 0) and (uuid in [uuid for uuid, _ in self._safe_remote_iterator()]) ): - if ( - self.connected_remote_pstop_state[uuid].connection_status.status - == PStopRemoteStatusEnum.DEACTIVATED - ): + if self.connected_remote_pstop_state[uuid].connection_status.status == PStopRemoteStatusEnum.DEACTIVATED: self.get_logger().warn( - f"Received message from {uuid} while not in active state", + f'Received message from {uuid} while not in active state', throttle_duration_sec=1.0, ) return self.connected_remote_pstop_state[uuid].pstop_pressed = msg.pstop_pressed - self.connected_remote_pstop_state[uuid].receive_timestamp = Ros2Time.from_ros_time( - self.get_clock().now() - ) - self.connected_remote_pstop_state[uuid].remote_timestamp = Ros2Time.from_time_msg( - msg.stamp - ) - self.connected_remote_pstop_state[uuid].counter = ( - self.connected_remote_pstop_state[uuid].counter + 1 - ) + self.connected_remote_pstop_state[uuid].receive_timestamp = Ros2Time.from_ros_time(self.get_clock().now()) + self.connected_remote_pstop_state[uuid].remote_timestamp = Ros2Time.from_time_msg(msg.stamp) + self.connected_remote_pstop_state[uuid].counter = self.connected_remote_pstop_state[uuid].counter + 1 reply_msg = ProtectiveStop() reply_msg.version = 0 reply_msg.pstop_pressed = self.connected_remote_pstop_state[uuid].pstop_pressed - reply_msg.connection_status = self.connected_remote_pstop_state[ - uuid - ].connection_status.to_ros_message() + reply_msg.connection_status = self.connected_remote_pstop_state[uuid].connection_status.to_ros_message() reply_msg.sender_uuid = self.machine_uuid reply_msg.receiver_uuid = uuid reply_msg.counter = self.connected_remote_pstop_state[uuid].counter reply_msg.stamp = self.get_clock().now().to_msg() self.get_logger().info( - f"Sending reply to {uuid}. P-Stop Pressed: {reply_msg.pstop_pressed}. Counter: {reply_msg.counter}", + f'Sending reply to {uuid}. P-Stop Pressed: {reply_msg.pstop_pressed}. Counter: {reply_msg.counter}', throttle_duration_sec=1.0, ) self.publisher_pstop.publish(reply_msg) else: - self.get_logger().warn( - f"Received message from unknown source: {uuid}", throttle_duration_sec=1.0 - ) + self.get_logger().warn(f'Received message from unknown source: {uuid}', throttle_duration_sec=1.0) def _has_exceeded_heartbeat_timeout(self, receive_timestamp: Ros2Time): timeout_adjusted_time = receive_timestamp.to_ros_time(self.get_clock()) + Duration( @@ -376,7 +335,7 @@ def _has_exceeded_heartbeat_timeout(self, receive_timestamp: Ros2Time): if has_exceeded: diff = now - timeout_adjusted_time self.get_logger().warn( - f"Exceeded heartbeat of {self.heartbeat_timeout}s. Diff: {diff.nanoseconds / 1e9}s", + f'Exceeded heartbeat of {self.heartbeat_timeout}s. Diff: {diff.nanoseconds / 1e9}s', throttle_duration_sec=1.0, ) return has_exceeded @@ -390,7 +349,7 @@ def _has_exceeded_deactivation_timeout(self, receive_timestamp: Ros2Time): if has_exceeded: diff = now - timeout_adjusted_time self.get_logger().warn( - f"Exceeded deactivation timeout of {self.deactivation_timeout}s. Diff: {diff.nanoseconds / 1e9}s", + f'Exceeded deactivation timeout of {self.deactivation_timeout}s. Diff: {diff.nanoseconds / 1e9}s', throttle_duration_sec=1.0, ) return has_exceeded @@ -411,32 +370,30 @@ def process_heartbeat(self): stop = None if self.is_user_monitored else False # Find all non-deactivated pstops - if not len( - [ - pstop - for _, pstop in self._safe_remote_iterator() - if pstop.connection_status.status != PStopRemoteStatusEnum.DEACTIVATED - ] - ): - self.get_logger().info("No connected remote pstops...", throttle_duration_sec=1.0) + if not len([ + pstop + for _, pstop in self._safe_remote_iterator() + if pstop.connection_status.status != PStopRemoteStatusEnum.DEACTIVATED + ]): + self.get_logger().info('No connected remote pstops...', throttle_duration_sec=1.0) for uuid, pstop_remote in self._safe_remote_iterator(): if pstop_remote.connection_status.status == PStopRemoteStatusEnum.DEACTIVATED: continue self.get_logger().debug( - f"Checking heartbeat for {uuid}. Status: {self._get_status_string(pstop_remote.connection_status)}. P-Stop Pressed: {pstop_remote.pstop_pressed}", + f'Checking heartbeat for {uuid}. Status: {self._get_status_string(pstop_remote.connection_status)}. P-Stop Pressed: {pstop_remote.pstop_pressed}', throttle_duration_sec=1.0, ) if self._has_exceeded_deactivation_timeout(pstop_remote.receive_timestamp): self.get_logger().info( colored( f"Deactivating remote '{uuid}'", - "cyan", + 'cyan', ), throttle_duration_sec=1.0, ) self.connected_remote_pstop_state[uuid].connection_status = ConnectionStatus( - status=PStopRemoteStatusEnum.DEACTIVATED, message="Deactivated due to timeout" + status=PStopRemoteStatusEnum.DEACTIVATED, message='Deactivated due to timeout' ) continue @@ -445,27 +402,24 @@ def process_heartbeat(self): self.get_logger().warn( colored( f"Missed heartbeat from remote '{uuid}'. Status: {PStopRemoteStatusEnum.UNSTABLE.name}...", - "yellow", + 'yellow', ), throttle_duration_sec=1.0, ) self.connected_remote_pstop_state[uuid].connection_status = ConnectionStatus( - status=PStopRemoteStatusEnum.UNSTABLE, message="Missed heartbeat" + status=PStopRemoteStatusEnum.UNSTABLE, message='Missed heartbeat' ) continue - elif ( - self.connected_remote_pstop_state[uuid].connection_status.status - == PStopRemoteStatusEnum.UNSTABLE - ): + elif self.connected_remote_pstop_state[uuid].connection_status.status == PStopRemoteStatusEnum.UNSTABLE: self.get_logger().info( colored( f"Recovered from unstable state for remote '{uuid}'", - "cyan", + 'cyan', ), ) self.connected_remote_pstop_state[uuid].connection_status = ConnectionStatus( - status=PStopRemoteStatusEnum.ACTIVE, message="" + status=PStopRemoteStatusEnum.ACTIVE, message='' ) stop = self.connected_remote_pstop_state[uuid].pstop_pressed @@ -474,7 +428,7 @@ def process_heartbeat(self): hb_msg.stamp = self.get_clock().now().to_msg() self.get_logger().debug( - f"Sending heartbeat. {colored(f'Stop: {hb_msg.stop}', 'cyan')}", + f'Sending heartbeat. {colored(f"Stop: {hb_msg.stop}", "cyan")}', ) self.publisher_hb.publish(hb_msg) @@ -492,15 +446,15 @@ def _publish_debug_msg(self): def main(args=None): rclpy.init(args=args) - protective_stop_node = ProtectiveStopNode("protective_stop_node") + protective_stop_node = ProtectiveStopNode('protective_stop_node') try: rclpy.spin(protective_stop_node) except KeyboardInterrupt: - protective_stop_node.get_logger().info("KeyboardInterrupt received, shutting down node") + protective_stop_node.get_logger().info('KeyboardInterrupt received, shutting down node') except SystemExit: - protective_stop_node.get_logger().info("SystemExit received, shutting down node") + protective_stop_node.get_logger().info('SystemExit received, shutting down node') except Exception as e: - protective_stop_node.get_logger().error("An error occurred: %s" % str(e)) + protective_stop_node.get_logger().error('An error occurred: %s' % str(e)) protective_stop_node.get_logger().error(traceback.format_exc()) finally: protective_stop_node.destroy_node() diff --git a/protective_stop_node/protective_stop_node/test_utils/__init__.py b/protective_stop_node/protective_stop_node/test_utils/__init__.py index 8b13789..e69de29 100644 --- a/protective_stop_node/protective_stop_node/test_utils/__init__.py +++ b/protective_stop_node/protective_stop_node/test_utils/__init__.py @@ -1 +0,0 @@ - diff --git a/protective_stop_node/protective_stop_node/test_utils/base_test.py b/protective_stop_node/protective_stop_node/test_utils/base_test.py index a9efcdd..97a7847 100644 --- a/protective_stop_node/protective_stop_node/test_utils/base_test.py +++ b/protective_stop_node/protective_stop_node/test_utils/base_test.py @@ -1,21 +1,20 @@ -#!/usr/bin/env python3 import threading import time import unittest import uuid import rclpy + from protective_stop_msg.msg import ( ProtectiveStop, ProtectiveStopDebug, ProtectiveStopHeartbeat, ) from protective_stop_msg.srv import ProtectiveStop as ProtectiveStopSrv - from protective_stop_node.protective_stop_node import ( - PROTECTIVE_STOP_TOPIC, PROTECTIVE_STOP_DEBUG_TOPIC, PROTECTIVE_STOP_HB_TOPIC, + PROTECTIVE_STOP_TOPIC, ) from protective_stop_node.test_utils.helpers import build_pstop_message @@ -24,7 +23,7 @@ class BaseTestProtectiveStopNode(unittest.TestCase): """Base test class for protective stop node tests.""" # These should be set by the child class or test module - machine_uuid = "machine-uuid" + machine_uuid = 'machine-uuid' TEST_HEARTBEAT_TIMEOUT_S = 0.1 TEST_DEACTIVATION_TIMEOUT_S = 3.0 TIMEOUT_PADDING = 0.05 @@ -44,29 +43,25 @@ def setUp(self): self.uuid = str(uuid.uuid4()) self.node = rclpy.create_node( - "test_node", + 'test_node', ) - params = self.node.get_parameters(["use_sim_time"]) + params = self.node.get_parameters(['use_sim_time']) if params: use_sim_time_value = params[0].value - print("use_sim_time:", use_sim_time_value) + print('use_sim_time:', use_sim_time_value) self.activating_msgs = [] self.pstop_msgs = [] self.pstop_hb_msgs = [] self.pstop_debug_msgs = [] - self.activate_client = self.node.create_client( - ProtectiveStopSrv, "/protective_stop_node/activate" - ) + self.activate_client = self.node.create_client(ProtectiveStopSrv, '/protective_stop_node/activate') if not self.activate_client.wait_for_service(timeout_sec=5.0): - self.fail("Service /protective_stop_node/activate not available") + self.fail('Service /protective_stop_node/activate not available') - self.deactivate_client = self.node.create_client( - ProtectiveStopSrv, "/protective_stop_node/deactivate" - ) + self.deactivate_client = self.node.create_client(ProtectiveStopSrv, '/protective_stop_node/deactivate') if not self.deactivate_client.wait_for_service(timeout_sec=5.0): - self.fail("Service /protective_stop_node/deactivate not available") + self.fail('Service /protective_stop_node/deactivate not available') self.pstop_publisher = self.node.create_publisher( ProtectiveStop, @@ -121,7 +116,7 @@ def _activate(self, uuid=None): rclpy.spin_until_future_complete(self.node, future) response = future.result() - self.assertIsNotNone(response, "No response received from /state_transition_service") + self.assertIsNotNone(response, 'No response received from /state_transition_service') self.assertTrue(response.success) def _deactivate(self, uuid=None): @@ -133,13 +128,13 @@ def _deactivate(self, uuid=None): rclpy.spin_until_future_complete(self.node, future) response = future.result() - self.assertIsNotNone(response, "No response received from /state_transition_service") + self.assertIsNotNone(response, 'No response received from /state_transition_service') def _spin_till_ok(self, timeout=5.0): now = time.time() while self.pstop_hb_msgs[-1].stop if self.pstop_hb_msgs else True: if time.time() - now > timeout: - self.fail("Timeout exceeded") + self.fail('Timeout exceeded') self.pstop_publisher.publish(build_pstop_message(self.machine_uuid, self.uuid, False)) time.sleep(self.TEST_HEARTBEAT_TIMEOUT_S - self.TIMEOUT_PADDING) rclpy.spin_once(self.node, timeout_sec=0.1) diff --git a/protective_stop_node/protective_stop_node/test_utils/helpers.py b/protective_stop_node/protective_stop_node/test_utils/helpers.py index 6560fd9..91e49c2 100644 --- a/protective_stop_node/protective_stop_node/test_utils/helpers.py +++ b/protective_stop_node/protective_stop_node/test_utils/helpers.py @@ -1,8 +1,8 @@ -#!/usr/bin/env python3 import time + from builtin_interfaces.msg import Time -from protective_stop_msg.msg import ProtectiveStop +from protective_stop_msg.msg import ProtectiveStop PSTOP_MESSAGE_VERSION = 0 @@ -10,13 +10,13 @@ def build_pstop_message(receiver_uuid, sender_uuid, pstop_pressed, counter=0): """ Build a ProtectiveStop message for testing purposes. - + Args: receiver_uuid: UUID of the receiver sender_uuid: UUID of the sender pstop_pressed: Boolean indicating if pstop is pressed counter: Message counter (default: 0) - + Returns: ProtectiveStop message """ @@ -29,7 +29,7 @@ def build_pstop_message(receiver_uuid, sender_uuid, pstop_pressed, counter=0): sender_uuid=sender_uuid, receiver_uuid=receiver_uuid, counter=counter, - checksum_type="CRC-16", + checksum_type='CRC-16', ) - return msg \ No newline at end of file + return msg diff --git a/protective_stop_node/test/launch.test.py b/protective_stop_node/test/launch.test.py index c415f34..db27fb9 100644 --- a/protective_stop_node/test/launch.test.py +++ b/protective_stop_node/test/launch.test.py @@ -1,24 +1,24 @@ # !/usr/bin/env python3 -import pytest -import launch_testing.util -import launch_testing.actions +import time +import uuid + import launch_testing +import launch_testing.actions +import launch_testing.util +import pytest +import rclpy +from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathSubstitution from launch_ros.substitutions import FindPackageShare -from launch import LaunchDescription -from protective_stop_msg.srv import ProtectiveStop as ProtectiveStopSrv - from rcl_interfaces.srv import SetParameters + +from protective_stop_msg.srv import ProtectiveStop as ProtectiveStopSrv from protective_stop_node.models import PStopRemoteStatusEnum -from protective_stop_node.test_utils.helpers import build_pstop_message from protective_stop_node.test_utils.base_test import BaseTestProtectiveStopNode -import rclpy -import time -import uuid - +from protective_stop_node.test_utils.helpers import build_pstop_message -machine_uuid = "machine-uuid" +machine_uuid = 'machine-uuid' TEST_HEARTBEAT_TIMEOUT_S = 0.1 TEST_DEACTIVATION_TIMEOUT_S = 3.0 TIMEOUT_PADDING = 0.05 @@ -27,22 +27,18 @@ @pytest.mark.launch_test def generate_test_description(): - return LaunchDescription( - [ - IncludeLaunchDescription( - PathSubstitution(FindPackageShare("protective_stop_node")) - / "launch" - / "protective_stop_node.launch.yaml", - launch_arguments={ - "machine_uuid": machine_uuid, - "heartbeat_timeout": str(TEST_HEARTBEAT_TIMEOUT_S), - "deactivation_timeout": str(TEST_DEACTIVATION_TIMEOUT_S), - "max_pstop_count": str(MAX_PSTOP_COUNT), - }.items(), - ), - launch_testing.actions.ReadyToTest(), - ] - ) + return LaunchDescription([ + IncludeLaunchDescription( + PathSubstitution(FindPackageShare('protective_stop_node')) / 'launch' / 'protective_stop_node.launch.yaml', + launch_arguments={ + 'machine_uuid': machine_uuid, + 'heartbeat_timeout': str(TEST_HEARTBEAT_TIMEOUT_S), + 'deactivation_timeout': str(TEST_DEACTIVATION_TIMEOUT_S), + 'max_pstop_count': str(MAX_PSTOP_COUNT), + }.items(), + ), + launch_testing.actions.ReadyToTest(), + ]) """ @@ -80,9 +76,9 @@ def test_err_uuid_mismatch_state_transition_service(self): rclpy.spin_until_future_complete(self.node, future) response = future.result() - self.assertIsNotNone(response, "No response received from /state_transition_service") + self.assertIsNotNone(response, 'No response received from /state_transition_service') self.assertFalse(response.success) - self.assertEqual(response.message, "Unknown receiver uuid") + self.assertEqual(response.message, 'Unknown receiver uuid') self.assertEqual(response.params.machine_uuid, machine_uuid) def test_err_deactivating_before_activating(self): @@ -94,9 +90,9 @@ def test_err_deactivating_before_activating(self): rclpy.spin_until_future_complete(self.node, future) response = future.result() - self.assertIsNotNone(response, "No response received from /state_transition_service") + self.assertIsNotNone(response, 'No response received from /state_transition_service') self.assertFalse(response.success) - self.assertEqual(response.message, "Deactivation failed. UUID not in connected list") + self.assertEqual(response.message, 'Deactivation failed. UUID not in connected list') def test_multiple_activations_deactivations(self): for _ in range(MAX_PSTOP_COUNT): @@ -118,29 +114,25 @@ def test_presses_pstop(self): self._activate() self._spin_till_ok() - self.assertFalse(self.pstop_hb_msgs[-1].stop, "Protective stop bool not set to true") + self.assertFalse(self.pstop_hb_msgs[-1].stop, 'Protective stop bool not set to true') PSTOP_PRESSED = True while not self.pstop_hb_msgs[-1].stop: - self.pstop_publisher.publish( - build_pstop_message(machine_uuid, self.uuid, PSTOP_PRESSED) - ) + self.pstop_publisher.publish(build_pstop_message(machine_uuid, self.uuid, PSTOP_PRESSED)) time.sleep(TEST_HEARTBEAT_TIMEOUT_S - TIMEOUT_PADDING) rclpy.spin_once(self.node, timeout_sec=0.1) - self.assertTrue(self.pstop_hb_msgs[-1].stop, "Protective stop bool not set to false") + self.assertTrue(self.pstop_hb_msgs[-1].stop, 'Protective stop bool not set to false') PSTOP_PRESSED = False while self.pstop_hb_msgs[-1].stop: - self.pstop_publisher.publish( - build_pstop_message(machine_uuid, self.uuid, PSTOP_PRESSED) - ) + self.pstop_publisher.publish(build_pstop_message(machine_uuid, self.uuid, PSTOP_PRESSED)) time.sleep(TEST_HEARTBEAT_TIMEOUT_S - TIMEOUT_PADDING) rclpy.spin_once(self.node, timeout_sec=0.1) - self.assertFalse(self.pstop_hb_msgs[-1].stop, "Protective stop bool not set to false") + self.assertFalse(self.pstop_hb_msgs[-1].stop, 'Protective stop bool not set to false') self._deactivate() @@ -187,7 +179,6 @@ def test_deactivates_after_set_timeout(self): time.sleep(TEST_HEARTBEAT_TIMEOUT_S + 1.0) rclpy.spin_once(self.node, timeout_sec=0.1) - time.sleep(TEST_DEACTIVATION_TIMEOUT_S) # Should be able to re-activate since it has been deactivated internally @@ -198,16 +189,17 @@ def test_setting_unmonitored_mode(self): # Setup params client self.set_params_srv = self.node.create_client(SetParameters, '/protective_stop_node/set_parameters') if not self.set_params_srv.wait_for_service(timeout_sec=5.0): - self.fail("Service /protective_stop_node/set_parameters not available") + self.fail('Service /protective_stop_node/set_parameters not available') - future = self.set_params_srv.call_async(SetParameters.Request( - parameters=[ - rclpy.parameter.Parameter("is_user_monitored", rclpy.Parameter.Type.BOOL, False).to_parameter_msg() - ] - )) + future = self.set_params_srv.call_async( + SetParameters.Request( + parameters=[ + rclpy.parameter.Parameter('is_user_monitored', rclpy.Parameter.Type.BOOL, False).to_parameter_msg() + ] + ) + ) rclpy.spin_until_future_complete(self.node, future) - # Wait for a heartbeat with stop == False timeout = 5.0 start_time = time.time() @@ -218,11 +210,13 @@ def test_setting_unmonitored_mode(self): self.assertFalse(self.pstop_hb_msgs[-1].stop) # test re-enable pstop - future = self.set_params_srv.call_async(SetParameters.Request( - parameters=[ - rclpy.parameter.Parameter("is_user_monitored", rclpy.Parameter.Type.BOOL, True).to_parameter_msg() - ] - )) + future = self.set_params_srv.call_async( + SetParameters.Request( + parameters=[ + rclpy.parameter.Parameter('is_user_monitored', rclpy.Parameter.Type.BOOL, True).to_parameter_msg() + ] + ) + ) rclpy.spin_until_future_complete(self.node, future) # Wait for a heartbeat with stop == True diff --git a/protective_stop_node/test/unmonitored_mode.test.py b/protective_stop_node/test/unmonitored_mode.test.py index 6823197..8204b8a 100644 --- a/protective_stop_node/test/unmonitored_mode.test.py +++ b/protective_stop_node/test/unmonitored_mode.test.py @@ -1,23 +1,23 @@ # !/usr/bin/env python3 -import pytest -import launch_testing.actions +import time + import launch_testing +import launch_testing.actions +import pytest +import rclpy from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathSubstitution from launch_ros.substitutions import FindPackageShare -from rcl_interfaces.srv import SetParameters from protective_stop_node.test_utils.base_test import BaseTestProtectiveStopNode -import rclpy -import time """ Separating out the unsafe mode tests to a separate file to keep the test file clean and unlikely to suffer any kind of cross-test pollution. """ -machine_uuid = "machine-uuid" +machine_uuid = 'machine-uuid' TEST_HEARTBEAT_TIMEOUT_S = 0.1 TEST_DEACTIVATION_TIMEOUT_S = 3.0 TIMEOUT_PADDING = 0.05 @@ -26,28 +26,24 @@ @pytest.mark.launch_test def generate_test_description(): - return LaunchDescription( - [ - IncludeLaunchDescription( - PathSubstitution(FindPackageShare("protective_stop_node")) - / "launch" - / "protective_stop_node.launch.yaml", - launch_arguments={ - "is_user_monitored": "False", - "machine_uuid": machine_uuid, - "heartbeat_timeout": str(TEST_HEARTBEAT_TIMEOUT_S), - "deactivation_timeout": str(TEST_DEACTIVATION_TIMEOUT_S), - "max_pstop_count": str(MAX_PSTOP_COUNT), - }.items(), - ), - launch_testing.actions.ReadyToTest(), - ] - ) + return LaunchDescription([ + IncludeLaunchDescription( + PathSubstitution(FindPackageShare('protective_stop_node')) / 'launch' / 'protective_stop_node.launch.yaml', + launch_arguments={ + 'is_user_monitored': 'False', + 'machine_uuid': machine_uuid, + 'heartbeat_timeout': str(TEST_HEARTBEAT_TIMEOUT_S), + 'deactivation_timeout': str(TEST_DEACTIVATION_TIMEOUT_S), + 'max_pstop_count': str(MAX_PSTOP_COUNT), + }.items(), + ), + launch_testing.actions.ReadyToTest(), + ]) class TestProtectiveStopNode(BaseTestProtectiveStopNode): def test_setting_unmonitored_mode(self): - # Wait for a heartbeat with stop == False + # Wait for a heartbeat with stop == False timeout = 5.0 start_time = time.time() while time.time() - start_time < timeout: diff --git a/protective_stop_remote_example/channel.json b/protective_stop_remote_example/channel.json index 9b0e124..2d3f695 100644 --- a/protective_stop_remote_example/channel.json +++ b/protective_stop_remote_example/channel.json @@ -5,4 +5,4 @@ "schemaEncoding": "ros2msg", "schemaName": "protective_stop_msg/msg/ProtectiveStop", "topic": "/protective_stop" -} \ No newline at end of file +} diff --git a/protective_stop_remote_example/public/vite.svg b/protective_stop_remote_example/public/vite.svg index e7b8dfb..ee9fada 100644 --- a/protective_stop_remote_example/public/vite.svg +++ b/protective_stop_remote_example/public/vite.svg @@ -1 +1 @@ - \ No newline at end of file + diff --git a/protective_stop_remote_example/src/ProtectiveStopMsgSchema.txt b/protective_stop_remote_example/src/ProtectiveStopMsgSchema.txt index c8682a5..487a268 100644 --- a/protective_stop_remote_example/src/ProtectiveStopMsgSchema.txt +++ b/protective_stop_remote_example/src/ProtectiveStopMsgSchema.txt @@ -49,4 +49,3 @@ MSG: unique_identifier_msgs/UUID # http://tools.ietf.org/html/rfc4122.html uint8[16] uuid - diff --git a/protective_stop_remote_example/src/ProtectiveStopSrvResponseSchema.txt b/protective_stop_remote_example/src/ProtectiveStopSrvResponseSchema.txt index ea86ce3..83d5fcd 100644 --- a/protective_stop_remote_example/src/ProtectiveStopSrvResponseSchema.txt +++ b/protective_stop_remote_example/src/ProtectiveStopSrvResponseSchema.txt @@ -144,4 +144,3 @@ MSG: unique_identifier_msgs/UUID # http://tools.ietf.org/html/rfc4122.html uint8[16] uuid - diff --git a/protective_stop_remote_example/src/assets/react.svg b/protective_stop_remote_example/src/assets/react.svg index 6c87de9..8e0e0f1 100644 --- a/protective_stop_remote_example/src/assets/react.svg +++ b/protective_stop_remote_example/src/assets/react.svg @@ -1 +1 @@ - \ No newline at end of file + diff --git a/protective_stop_remote_example/src/test/ws-server.ts b/protective_stop_remote_example/src/test/ws-server.ts index 607e5fc..4816bfd 100644 --- a/protective_stop_remote_example/src/test/ws-server.ts +++ b/protective_stop_remote_example/src/test/ws-server.ts @@ -90,4 +90,4 @@ async function setupServerAndClient(server: FoxgloveServer) { wss.close(); }; return { server, send, nextJsonMessage, nextBinaryMessage, nextEvent, close }; -} \ No newline at end of file +} diff --git a/pstop_c/CMakeLists.txt b/pstop_c/CMakeLists.txt index 01373a0..ac3bcc4 100644 --- a/pstop_c/CMakeLists.txt +++ b/pstop_c/CMakeLists.txt @@ -2,4 +2,3 @@ cmake_minimum_required(VERSION 3.12) project(PSTOP C) add_subdirectory(pstop) - diff --git a/pstop_c/pstop/include/pstop/pstop_msg.h b/pstop_c/pstop/include/pstop/pstop_msg.h index 1592f18..64c2ca7 100644 --- a/pstop_c/pstop/include/pstop/pstop_msg.h +++ b/pstop_c/pstop/include/pstop/pstop_msg.h @@ -76,4 +76,3 @@ uint16_t pstop_calculate_checksum(const pstop_msg_t *msg); pstop_error_t pstop_is_message_valid(const pstop_msg_t *msg); #endif /* PSTOP_PSTOP_MSG_H */ - diff --git a/pstop_c/pstop/include/pstop/time.h b/pstop_c/pstop/include/pstop/time.h index 8c52d38..bd03723 100644 --- a/pstop_c/pstop/include/pstop/time.h +++ b/pstop_c/pstop/include/pstop/time.h @@ -6,4 +6,3 @@ uint64_t time_get_now(void); #endif /* PSTOP_TIME_H */ - diff --git a/pstop_c/pstop/src/pstop/time.c b/pstop_c/pstop/src/pstop/time.c index 24bb9fd..4c4936b 100644 --- a/pstop_c/pstop/src/pstop/time.c +++ b/pstop_c/pstop/src/pstop/time.c @@ -16,4 +16,3 @@ time_get_now(void) return 0U; #endif } - diff --git a/pstop_c/pstop/test/src/pstop/machine_test.c b/pstop_c/pstop/test/src/pstop/machine_test.c index 766b4c7..4451bde 100644 --- a/pstop_c/pstop/test/src/pstop/machine_test.c +++ b/pstop_c/pstop/test/src/pstop/machine_test.c @@ -496,4 +496,4 @@ main_machine_test(void) RUN_TEST(test_2_clients_stop_unbond); RUN_TEST(test_bond_ok); RUN_TEST(test_bond_stop_ok); -} \ No newline at end of file +} diff --git a/pstop_c/pstop/test/src/pstop/machine_timeout_test.c b/pstop_c/pstop/test/src/pstop/machine_timeout_test.c index c4c5b33..7dd45cf 100644 --- a/pstop_c/pstop/test/src/pstop/machine_timeout_test.c +++ b/pstop_c/pstop/test/src/pstop/machine_timeout_test.c @@ -187,4 +187,4 @@ main_machine_timeout_test(void) RUN_TEST(test_bond_timeout); RUN_TEST(test_bond_stop_timeout); RUN_TEST(test_bond_stop_timeout_2_missed_timeouts); -} \ No newline at end of file +} diff --git a/pstop_c/pstop/test/src/pstop/main.c b/pstop_c/pstop/test/src/pstop/main.c index 42c10a6..a959dc0 100644 --- a/pstop_c/pstop/test/src/pstop/main.c +++ b/pstop_c/pstop/test/src/pstop/main.c @@ -21,4 +21,4 @@ main(void) main_machine_timeout_test(); return UNITY_END(); -} \ No newline at end of file +} diff --git a/pstop_c/pstop/test/src/pstop/pstop_client_test.c b/pstop_c/pstop/test/src/pstop/pstop_client_test.c index 2a55148..0a4e8c4 100644 --- a/pstop_c/pstop/test/src/pstop/pstop_client_test.c +++ b/pstop_c/pstop/test/src/pstop/pstop_client_test.c @@ -130,4 +130,4 @@ main_pstop_client_test(void) RUN_TEST(test_get_free_client); RUN_TEST(test_remove_client); -} \ No newline at end of file +} diff --git a/pstop_c/pstop/test/src/pstop/pstop_tests.yml b/pstop_c/pstop/test/src/pstop/pstop_tests.yml index 717e5ab..148905c 100644 --- a/pstop_c/pstop/test/src/pstop/pstop_tests.yml +++ b/pstop_c/pstop/test/src/pstop/pstop_tests.yml @@ -1,3 +1,4 @@ +--- :unity: :main_name: auto - :framework: 'unity/unity' \ No newline at end of file + :framework: unity/unity diff --git a/pstop_c/pstop/test/src/pstop/test_utils.c b/pstop_c/pstop/test/src/pstop/test_utils.c index bde457c..3843bdb 100644 --- a/pstop_c/pstop/test/src/pstop/test_utils.c +++ b/pstop_c/pstop/test/src/pstop/test_utils.c @@ -16,4 +16,3 @@ device_id_set(device_id_t *device_id, const char *id) memcpy(device_id->data, id, DEVICE_ID_LENGTH); } } - From ef7c1367c21bab8639d9b4af57a9bb4476e046d6 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 28 Apr 2026 16:40:08 -0700 Subject: [PATCH 2/5] Apply markdown and copyright fixups --- .pre-commit-config.yaml | 4 +- archive/docs/Robot-Quickstart.md | 76 ++-- archive/docs/Software.md | 416 ++++++++++-------- .../protective_stop_remote/__init__.py | 2 + .../protective_stop_remote/app.py | 2 + .../protective_stop_remote/button.py | 2 + .../protective_stop_remote/main.py | 2 + .../protective_stop_remote/power.py | 2 + .../protective_stop_remote/pstop.py | 2 + .../protective_stop_remote/ui.py | 2 + archive/protective_stop_remote/setup.py | 2 + .../protective_stop_remote/tests/__init__.py | 2 + .../protective_stop_remote/tests/test_app.py | 2 + protective_stop_msg/CMakeLists.txt | 2 + protective_stop_node/CMakeLists.txt | 2 + .../protective_stop_node/__init__.py | 2 + .../protective_stop_node/models.py | 2 + .../protective_stop_node.py | 2 + .../test_utils/__init__.py | 2 + .../test_utils/base_test.py | 2 + .../test_utils/helpers.py | 2 + protective_stop_node/test/launch.test.py | 2 + .../test/unmonitored_mode.test.py | 2 + pstop_c/CMakeLists.txt | 2 + pstop_c/pstop/CMakeLists.txt | 2 + pstop_c/pstop/include/pstop/checksum.h | 3 + pstop_c/pstop/include/pstop/constants.h | 3 + pstop_c/pstop/include/pstop/device_id.h | 3 + pstop_c/pstop/include/pstop/error.h | 3 + pstop_c/pstop/include/pstop/machine.h | 3 + pstop_c/pstop/include/pstop/protocol.h | 3 + .../pstop/include/pstop/pstop_application.h | 3 + pstop_c/pstop/include/pstop/pstop_client.h | 3 + pstop_c/pstop/include/pstop/pstop_msg.h | 3 + pstop_c/pstop/include/pstop/time.h | 3 + pstop_c/pstop/src/pstop/checksum.c | 3 + pstop_c/pstop/src/pstop/device_id.c | 3 + pstop_c/pstop/src/pstop/machine.c | 3 + pstop_c/pstop/src/pstop/protocol.c | 3 + pstop_c/pstop/src/pstop/pstop_application.c | 3 + pstop_c/pstop/src/pstop/pstop_client.c | 3 + pstop_c/pstop/src/pstop/pstop_msg.c | 3 + pstop_c/pstop/src/pstop/time.c | 3 + pstop_c/pstop/test/include/pstop/test_utils.h | 3 + pstop_c/pstop/test/src/pstop/device_id_test.c | 3 + pstop_c/pstop/test/src/pstop/machine_test.c | 3 + .../test/src/pstop/machine_timeout_test.c | 3 + pstop_c/pstop/test/src/pstop/main.c | 3 + .../pstop/test/src/pstop/pstop_client_test.c | 3 + pstop_c/pstop/test/src/pstop/test_utils.c | 3 + 50 files changed, 390 insertions(+), 225 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 0ad13cc..89b9f90 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,8 +1,8 @@ --- # Apply Polymath Code Standard formatters and linters repos: - - repo: git@github.com:polymathrobotics/polymath_code_standard.git - rev: v2.1.0 + - repo: ~/dev/polymath/infra/polymath_code_standard + rev: feafa20be0c307cccb867209885ce4f83a34e9ce hooks: # Everything - id: polymath-general diff --git a/archive/docs/Robot-Quickstart.md b/archive/docs/Robot-Quickstart.md index cee2495..d6912cb 100644 --- a/archive/docs/Robot-Quickstart.md +++ b/archive/docs/Robot-Quickstart.md @@ -1,45 +1,59 @@ # Robot Quick-Start + Have a protective stop and want to set up your robot to work with it? Follow these instructions. ## Install Tailscale + 1. On your robot, install Tailscale on it with the following commands. You will be provided a link to log in to your Tailscale account after the last step. -```bash -curl -fsSL https://pkgs.tailscale.com/stable/ubuntu/focal.noarmor.gpg | sudo tee /usr/share/keyrings/tailscale-archive-keyring.gpg >/dev/null -curl -fsSL https://pkgs.tailscale.com/stable/ubuntu/focal.tailscale-keyring.list | sudo tee /etc/apt/sources.list.d/tailscale.list -sudo apt-get update -sudo apt-get install tailscale -sudo tailscale up -``` -2. We can then check the Tailscale address of the robot with `tailscale ip -4`. + ```bash + curl -fsSL https://pkgs.tailscale.com/stable/ubuntu/focal.noarmor.gpg | sudo tee /usr/share/keyrings/tailscale-archive-keyring.gpg >/dev/null + curl -fsSL https://pkgs.tailscale.com/stable/ubuntu/focal.tailscale-keyring.list | sudo tee /etc/apt/sources.list.d/tailscale.list + sudo apt-get update + sudo apt-get install tailscale + sudo tailscale up + ``` + +1. We can then check the Tailscale address of the robot with `tailscale ip -4`. ## roslibpy Custom Message Set-up + On your local machine (or robot that you want the p-stop to control): 1. Install rosbridge-server with `sudo apt-get install ros-$ROS-DISTRO-rosbridge-server`. -2. Create a colcon workspace if you don't already have one: -``` -mkdir -p colcon_ws/src -cd colcon_ws/src -``` -3. Put the [`pstop_msg`](../pstop_msg) folder in this repo inside the `src` directory. It contains a custom message definition for our protective stop. -4. Go back to the `colcon_ws` directory and run: -``` -colcon build -source install/setup.bash -``` -5. To verify that our custom message set-up was successful, you can run `ros2 interface show pstop_msg/msg/EStopMsg`, which should print out the message definition. +1. Create a colcon workspace if you don't already have one: + + ``` + mkdir -p colcon_ws/src + cd colcon_ws/src + ``` + +1. Put the [`pstop_msg`](../pstop_msg) folder in this repo inside the `src` directory. It contains a custom message definition for our protective stop. +1. Go back to the `colcon_ws` directory and run: + + ``` + colcon build + source install/setup.bash + ``` + +1. To verify that our custom message set-up was successful, you can run `ros2 interface show pstop_msg/msg/EStopMsg`, which should print out the message definition. ## roslibpy Client Set-up + In a terminal on your local machine where you have built and sourced the custom message type above: + 1. Launch a rosbridge_server with `ros2 launch rosbridge_server rosbridge_websocket_launch.xml`. -- If you are running this in a Docker container, make sure that port 9090 is exposed by doing `docker run -p 9090:9090 ...` -2. To run the roslibpy client, run `python roslibpy_client.py [ip address]`. -- You can set a default ip address by altering line 12 of [`roslibpy_client.py`](../roslibpy_client.py): -``` -parser.add_argument('target', type=str, help='Target IP address', default='') -``` -3. To use the flask interface, connect to the Raspberry Pi through SSH with portforwarding: -``` -ssh -L 8000:localhost:5000 [username]@[tailscale ip] -``` -- Now, you should be able to see the flask interface by going to `http://localhost:8000/config`. + - If you are running this in a Docker container, make sure that port 9090 is exposed by doing `docker run -p 9090:9090 ...` +1. To run the roslibpy client, run `python roslibpy_client.py [ip address]`. + - You can set a default ip address by altering line 12 of [`roslibpy_client.py`](../roslibpy_client.py): + + ``` + parser.add_argument('target', type=str, help='Target IP address', default='') + ``` + +1. To use the flask interface, connect to the Raspberry Pi through SSH with portforwarding: + + ``` + ssh -L 8000:localhost:5000 [username]@[tailscale ip] + ``` + + - Now, you should be able to see the flask interface by going to `http://localhost:8000/config`. diff --git a/archive/docs/Software.md b/archive/docs/Software.md index 447d31e..d99143f 100644 --- a/archive/docs/Software.md +++ b/archive/docs/Software.md @@ -1,225 +1,257 @@ # Software/Firmware ## Imaging Raspberry Pi + 1. Basic Set-up -- Download [Raspberry Pi OS image](https://downloads.raspberrypi.com/raspios_lite_arm64/images/raspios_lite_arm64-2024-03-15/2024-03-15-raspios-bookworm-arm64-lite.img.xz?_gl=1*13v8f9s*_ga*MTc2NDY2NjEzLjE3MTIzNTk2OTQ.*_ga_22FD70LWDS*MTcxMzgyMDE2OC4zLjEuMTcxMzgyMDE3My4wLjAuMA..). -- Take a microSD card and insert it into a computer with [Raspberry Pi Imager](https://www.raspberrypi.com/software/). Use a microSD adapter if necessary. -- In the imager: - - For device, elect “Raspberry Pi Zero 2W”. - - For operating system, use “Custom” and use the image you just downloaded, which should be called `2024-03-15-raspios-bookworm-arm64-lite.img`. - - For storage, choose the microSD card you just inserted. -

-
Raspberry Pi Imager -

- -2. Custom settings — “General” -- After clicking next, there is a pop-up for OS customization. Click “Edit Settings”. -- Set a hostname, as well as a username and password -- Input WiFi credentials - note that you likely can't use a 5G network. -

-
Custom Settings - General -

- -3. Enable SSH -- Go to the “Services” tab and click Enable SSH. -- Use password authentication so other people can communicate with the Raspberry Pi other than you. -

-
Custom Settings - General -

- -4. Proceed with flashing the Pi — there will be a “write” and “verify” step. - -5. Remove microSD from your computer and insert it in the Raspberry Pi. Turn on power and wait a couple minutes for the Raspberry Pi to boot. - -6. You can use find the IP address of the Raspberry Pi by going on your computer and running `nmap -p 22 --open [your network range]`. -- Your network range will be something like `192.168.93.0/24`, and can be found by running `ifconfig` or `ip addr`. -- Alternatively, you can use the more aggressive `nmap -sV -p 22 192.168.93.0/24`, and identify the Raspberry Pi from its operating system. - -7. Once you’ve found the IP address of the Raspberry Pi, connect to it over SSH by using: `ssh [username]@[ip address]` -- Here `[username]` is the username you set during the configuration step before, and the `[ip address]` is the IP address of the Raspberry Pi you just found. -- You will be prompted for the password — this is the password set earlier during the OS customization step. + - Download [Raspberry Pi OS image](https://downloads.raspberrypi.com/raspios_lite_arm64/images/raspios_lite_arm64-2024-03-15/2024-03-15-raspios-bookworm-arm64-lite.img.xz?_gl=1*13v8f9s*_ga*MTc2NDY2NjEzLjE3MTIzNTk2OTQ.*_ga_22FD70LWDS*MTcxMzgyMDE2OC4zLjEuMTcxMzgyMDE3My4wLjAuMA..). + - Take a microSD card and insert it into a computer with [Raspberry Pi Imager](https://www.raspberrypi.com/software/). Use a microSD adapter if necessary. + - In the imager: + - For device, elect “Raspberry Pi Zero 2W”. + - For operating system, use “Custom” and use the image you just downloaded, which should be called `2024-03-15-raspios-bookworm-arm64-lite.img`. + - For storage, choose the microSD card you just inserted. +

+
Raspberry Pi Imager +

+ +1. Custom settings — “General” + - After clicking next, there is a pop-up for OS customization. Click “Edit Settings”. + - Set a hostname, as well as a username and password + - Input WiFi credentials - note that you likely can't use a 5G network. +

+
Custom Settings - General +

+ +1. Enable SSH + - Go to the “Services” tab and click Enable SSH. + - Use password authentication so other people can communicate with the Raspberry Pi other than you. +

+
Custom Settings - General +

+ +1. Proceed with flashing the Pi — there will be a “write” and “verify” step. + +1. Remove microSD from your computer and insert it in the Raspberry Pi. Turn on power and wait a couple minutes for the Raspberry Pi to boot. + +1. You can use find the IP address of the Raspberry Pi by going on your computer and running `nmap -p 22 --open [your network range]`. + - Your network range will be something like `192.168.93.0/24`, and can be found by running `ifconfig` or `ip addr`. + - Alternatively, you can use the more aggressive `nmap -sV -p 22 192.168.93.0/24`, and identify the Raspberry Pi from its operating system. + +1. Once you’ve found the IP address of the Raspberry Pi, connect to it over SSH by using: `ssh [username]@[ip address]` + - Here `[username]` is the username you set during the configuration step before, and the `[ip address]` is the IP address of the Raspberry Pi you just found. + - You will be prompted for the password — this is the password set earlier during the OS customization step. ## Raspberry Pi Software Installation + 1. After connecting to the Raspberry Pi through SSH, install Tailscale on it with the following commands. You will be provided a link to log in to your Tailscale account after the last step. -```bash -curl -fsSL https://pkgs.tailscale.com/stable/ubuntu/focal.noarmor.gpg | sudo tee /usr/share/keyrings/tailscale-archive-keyring.gpg >/dev/null -curl -fsSL https://pkgs.tailscale.com/stable/ubuntu/focal.tailscale-keyring.list | sudo tee /etc/apt/sources.list.d/tailscale.list -sudo apt-get update -sudo apt-get install tailscale -sudo tailscale up -``` -2. We can then check the Tailscale address of the Rasperry Pi with `tailscale ip -4`. Now, we can access the Raspberry Pi over VPN anywhere through Tailscale by using `ssh [username]@[tailscale ip]` - -3. Install the following packages on the Raspberry Pi: -- Install chrony with `sudo apt install chrony` -- Install pip with `sudo apt install python3-pip -y` -- Install PySerial with `sudo pip install pyserial --break-system-packages` -- Install the roslibpy with: -``` -sudo apt install git-all -git clone https://github.com/gramaziokohler/roslibpy.git -cd roslibpy -sudo pip install -r requirements-dev.txt --break-system-packages -``` -- For more information on roslibpy, refer to the [roslibpy repo](https://github.com/gramaziokohler/roslibpy). + + ```bash + curl -fsSL https://pkgs.tailscale.com/stable/ubuntu/focal.noarmor.gpg | sudo tee /usr/share/keyrings/tailscale-archive-keyring.gpg >/dev/null + curl -fsSL https://pkgs.tailscale.com/stable/ubuntu/focal.tailscale-keyring.list | sudo tee /etc/apt/sources.list.d/tailscale.list + sudo apt-get update + sudo apt-get install tailscale + sudo tailscale up + ``` + +1. We can then check the Tailscale address of the Rasperry Pi with `tailscale ip -4`. Now, we can access the Raspberry Pi over VPN anywhere through Tailscale by using `ssh [username]@[tailscale ip]` +1. Install the following packages on the Raspberry Pi: + - Install chrony with `sudo apt install chrony` + - Install pip with `sudo apt install python3-pip -y` + - Install PySerial with `sudo pip install pyserial --break-system-packages` + - Install the roslibpy with: + + ``` + sudo apt install git-all + git clone https://github.com/gramaziokohler/roslibpy.git + cd roslibpy + sudo pip install -r requirements-dev.txt --break-system-packages + ``` + + - For more information on roslibpy, refer to the [roslibpy repo](https://github.com/gramaziokohler/roslibpy). ## ESP32 Set-up + On the Raspberry Pi (over SSH): 1. Install esptool with `pip install esptool --break-system-packages` -2. Make sure these four files on Raspberry Pi, which are in the [pstop_MCU build folder](../pstop_MCU/build/esp32.esp32.esp32/) folder: -- `boot_app0.bin` -- `pstop_MCU.ino.bin` -- `pstop_MCU.ino.bootloader.bin` -- `pstop_MCU.ino.partitions.bin` -- If you are moving these files from your local computer, this can be done by going into the directory with these files and doing: -``` -scp esp32_new.ino.bin esp32_new.ino.bootloader.bin esp32_new.ino.partitions.bin [username]@[tailscale ip]:/path/to/home/dir -``` -- The original code [`pstop_MCU.ino`](../pstop_MCU/pstop_MCU.ino) file is also included, so you can make changes and re-compile if desired. -3. Now, run the [esp32_flash.sh](../esp32_flash.sh) bash script with `bash esp32_flash.sh`. This should output something like this: -``` -polymath@polymath-estop-001:~ $ bash esp32_flash.sh -esptool.py v4.7.0 -Serial port /dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0 -Connecting..... -Chip is ESP32-D0WD-V3 (revision v3.1) -Features: WiFi, BT, Dual Core, 240MHz, VRef calibration in efuse, Coding Scheme None -Crystal is 40MHz -MAC: e4:65:b8:0f:49:f4 -Uploading stub... -Running stub... -Stub running... -Changing baud rate to 921600 -Changed. -Configuring flash size... -Flash will be erased from 0x00001000 to 0x00005fff... -Flash will be erased from 0x00008000 to 0x00008fff... -Flash will be erased from 0x0000e000 to 0x0000ffff... -Flash will be erased from 0x00010000 to 0x000e4fff... -Compressed 18992 bytes to 13112... -Wrote 18992 bytes (13112 compressed) at 0x00001000 in 0.3 seconds (effective 583.1 kbit/s)... -Hash of data verified. -Compressed 3072 bytes to 146... -Wrote 3072 bytes (146 compressed) at 0x00008000 in 0.0 seconds (effective 1368.9 kbit/s)... -Hash of data verified. -Compressed 8192 bytes to 47... -Wrote 8192 bytes (47 compressed) at 0x0000e000 in 0.0 seconds (effective 2240.4 kbit/s)... -Hash of data verified. -Compressed 870384 bytes to 563721... -Wrote 870384 bytes (563721 compressed) at 0x00010000 in 7.2 seconds (effective 968.5 kbit/s)... -Hash of data verified. - -Leaving... -Hard resetting via RTS pin... -Flashing complete. -Resetting USB-to-UART bridge... -Bridge reset complete. -Configuring serial port settings... -Reading from serial port... -``` -4. After seeing "Reading from serial port", you can stop the script with Ctrl + C. -5. At this point, the LED ring should be lighting up and be responsive to the red stop button. +1. Make sure these four files on Raspberry Pi, which are in the [pstop_MCU build folder](../pstop_MCU/build/esp32.esp32.esp32/) folder: + - `boot_app0.bin` + - `pstop_MCU.ino.bin` + - `pstop_MCU.ino.bootloader.bin` + - `pstop_MCU.ino.partitions.bin` + - If you are moving these files from your local computer, this can be done by going into the directory with these files and doing: + + ``` + scp esp32_new.ino.bin esp32_new.ino.bootloader.bin esp32_new.ino.partitions.bin [username]@[tailscale ip]:/path/to/home/dir + ``` + + - The original code [`pstop_MCU.ino`](../pstop_MCU/pstop_MCU.ino) file is also included, so you can make changes and re-compile if desired. + +1. Now, run the [esp32_flash.sh](../esp32_flash.sh) bash script with `bash esp32_flash.sh`. This should output something like this: + + ``` + polymath@polymath-estop-001:~ $ bash esp32_flash.sh + esptool.py v4.7.0 + Serial port /dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0 + Connecting..... + Chip is ESP32-D0WD-V3 (revision v3.1) + Features: WiFi, BT, Dual Core, 240MHz, VRef calibration in efuse, Coding Scheme None + Crystal is 40MHz + MAC: e4:65:b8:0f:49:f4 + Uploading stub... + Running stub... + Stub running... + Changing baud rate to 921600 + Changed. + Configuring flash size... + Flash will be erased from 0x00001000 to 0x00005fff... + Flash will be erased from 0x00008000 to 0x00008fff... + Flash will be erased from 0x0000e000 to 0x0000ffff... + Flash will be erased from 0x00010000 to 0x000e4fff... + Compressed 18992 bytes to 13112... + Wrote 18992 bytes (13112 compressed) at 0x00001000 in 0.3 seconds (effective 583.1 kbit/s)... + Hash of data verified. + Compressed 3072 bytes to 146... + Wrote 3072 bytes (146 compressed) at 0x00008000 in 0.0 seconds (effective 1368.9 kbit/s)... + Hash of data verified. + Compressed 8192 bytes to 47... + Wrote 8192 bytes (47 compressed) at 0x0000e000 in 0.0 seconds (effective 2240.4 kbit/s)... + Hash of data verified. + Compressed 870384 bytes to 563721... + Wrote 870384 bytes (563721 compressed) at 0x00010000 in 7.2 seconds (effective 968.5 kbit/s)... + Hash of data verified. + + Leaving... + Hard resetting via RTS pin... + Flashing complete. + Resetting USB-to-UART bridge... + Bridge reset complete. + Configuring serial port settings... + Reading from serial port... + ``` + +1. After seeing "Reading from serial port", you can stop the script with Ctrl + C. +1. At this point, the LED ring should be lighting up and be responsive to the red stop button. For more information on esptool, refer to their [repo](https://github.com/espressif/esptool) or [docs](https://docs.espressif.com/projects/esptool/en/latest/esp32/). ## SIM Module Set-up + On the Raspberry Pi: 1. Install minicom with `sudo apt install minicom` -2. Start minimcom with `sudo systemctl stop ModemManager.service && minicom -D /dev/ttyUSB2` -- This may not work properly if `/dev/ttyUSB2` is not the right address. -- You can tell whether it's working properly by typing `ATE` and then Enter in the minicom terminal and seeing if there is an `OK` response. -- You may have to try some different addresses, which can be found with: -``` -cd /dev/serial/by-id -ls -``` -3. In the minicom terminal, issue the following commands: -``` -ATE -AT&F -ATI -AT&V -AT+CGDCONT? -AT+CUSBPIDSWITCH? -AT+CGPSAUTO? -AT+CUSBPIDSWITCH=9001,1,1 -AT+CGDCONT=1,"IPV4V6","h2g2" -AT+CGDCONT=6,"IPV4V6","h2g2" -AT+CGPSAUTO=1 -``` -4. Exit minicom (`Ctrl+A, Z, X`), and then power cycle the system. -5. Now, configure the Raspian Bookworm Network Manager with: `sudo nmcli connection add type gsm ifname '*' con-name '1-gsm' apn 'h2g2' connection.autoconnect yes` -6. If you need more detailed instructions, refer to https://wimsworld.wordpress.com/2023/12/ +1. Start minimcom with `sudo systemctl stop ModemManager.service && minicom -D /dev/ttyUSB2` + - This may not work properly if `/dev/ttyUSB2` is not the right address. + - You can tell whether it's working properly by typing `ATE` and then Enter in the minicom terminal and seeing if there is an `OK` response. + - You may have to try some different addresses, which can be found with: + + ``` + cd /dev/serial/by-id + ls + ``` + +1. In the minicom terminal, issue the following commands: + + ``` + ATE + AT&F + ATI + AT&V + AT+CGDCONT? + AT+CUSBPIDSWITCH? + AT+CGPSAUTO? + AT+CUSBPIDSWITCH=9001,1,1 + AT+CGDCONT=1,"IPV4V6","h2g2" + AT+CGDCONT=6,"IPV4V6","h2g2" + AT+CGPSAUTO=1 + ``` + +1. Exit minicom (`Ctrl+A, Z, X`), and then power cycle the system. +1. Now, configure the Raspian Bookworm Network Manager with: `sudo nmcli connection add type gsm ifname '*' con-name '1-gsm' apn 'h2g2' connection.autoconnect yes` +1. If you need more detailed instructions, refer to https://wimsworld.wordpress.com/2023/12/ ## E-ink Paper Display + On the Raspberry Pi terminal: 1. Install the following: -``` -sudo pip3 install RPi.GPIO -sudo pip3 install spidev -``` -2. Git clone the e-Paper repo with `git clone https://github.com/waveshare/e-Paper.git` -3. Go in the Raspberry Pi directory with: `cd e-Paper/RaspberryPi_JetsonNano/` -4. Set up the libraries with `sudo python3 [setup.py](http://setup.py/) install` -5. Install flask with `pip install flask --break-system-packages` -6. Configure the Raspberry Pi by typing `sudo raspi-config`. -- Go to `Choose Interfacing Options -> SPI -> Yes Enable SPI interface` -

-
Raspberry Pi Config Menu -

- -7. Reboot your Raspberry Pi with either `sudo reboot` or power cycle. + + ``` + sudo pip3 install RPi.GPIO + sudo pip3 install spidev + ``` + +1. Git clone the e-Paper repo with `git clone https://github.com/waveshare/e-Paper.git` +1. Go in the Raspberry Pi directory with: `cd e-Paper/RaspberryPi_JetsonNano/` +1. Set up the libraries with `sudo python3 [setup.py](http://setup.py/) install` +1. Install flask with `pip install flask --break-system-packages` +1. Configure the Raspberry Pi by typing `sudo raspi-config`. + - Go to `Choose Interfacing Options -> SPI -> Yes Enable SPI interface` +

+
Raspberry Pi Config Menu +

+ +1. Reboot your Raspberry Pi with either `sudo reboot` or power cycle. For more info on this display, please refer to Waveshare's [manual](https://www.waveshare.com/wiki/2.13inch_e-Paper_HAT_Manual). ## Custom Message Set-up + On your local machine your local machine or (robot that you want the p-stop to control): 1. Install rosbridge-server with `sudo apt-get install ros-$ROS-DISTRO-rosbridge-server`. -2. Create a colcon workspace if you don't already have one: -``` -mkdir -p colcon_ws/src -cd colcon_ws/src -``` -3. Put the [`pstop_msg`](../pstop_msg) folder in this repo inside the `src` directory. It contains a custom message definition for our protective stop. -4. Go back to the `colcon_ws` directory and run: -``` -colcon build -source install/setup.bash -``` -5. To verify that our custom message set-up was successful, you can run `ros2 interface show estop_interface/msg/EStopMsg`, which should print out the message definition. +1. Create a colcon workspace if you don't already have one: + + ``` + mkdir -p colcon_ws/src + cd colcon_ws/src + ``` + +1. Put the [`pstop_msg`](../pstop_msg) folder in this repo inside the `src` directory. It contains a custom message definition for our protective stop. +1. Go back to the `colcon_ws` directory and run: + + ``` + colcon build + source install/setup.bash + ``` + +1. To verify that our custom message set-up was successful, you can run `ros2 interface show estop_interface/msg/EStopMsg`, which should print out the message definition. ## roslibpy Client Set-up + In a terminal on your local machine where you have built and sourced the custom message type above: 1. Launch a rosbridge_server with `ros2 launch rosbridge_server rosbridge_websocket_launch.xml`. -- If you are running this in a Docker container, make sure that port 9090 is exposed by doing `docker run -p 9090:9090 ...` -2. To run the roslibpy client, run `python roslibpy_client.py [ip address]`. -- You can set a default ip address by altering line 12 of [`roslibpy_client.py`](../roslibpy_client.py): -``` -parser.add_argument('target', type=str, help='Target IP address', default='') -``` -3. To use the flask interface, connect to the Raspberry Pi through SSH with portforwarding: -``` -ssh -L 8000:localhost:5000 [username]@[tailscale ip] -``` -- Now, you should be able to see the flask interface by going to `http://localhost:8000/config`. -- For more information about roslibpy and rosbridge_server, see [Robot Web Tools](https://robotwebtools.github.io/). + - If you are running this in a Docker container, make sure that port 9090 is exposed by doing `docker run -p 9090:9090 ...` +1. To run the roslibpy client, run `python roslibpy_client.py [ip address]`. + - You can set a default ip address by altering line 12 of [`roslibpy_client.py`](../roslibpy_client.py): + + ``` + parser.add_argument('target', type=str, help='Target IP address', default='') + ``` + +1. To use the flask interface, connect to the Raspberry Pi through SSH with portforwarding: + + ``` + ssh -L 8000:localhost:5000 [username]@[tailscale ip] + ``` + + - Now, you should be able to see the flask interface by going to `http://localhost:8000/config`. + - For more information about roslibpy and rosbridge_server, see [Robot Web Tools](https://robotwebtools.github.io/). ## Starting on Boot + If you want to start the roslibpy client above on boot, we can do so with a systemctl service. 1. Create a service file with `sudo nano /etc/systemd/system/pstop-autostart.service`. -2. Edit the service file `nano` or your text editor of choice and add: -``` -[Unit] -Description=Start protective stop on boot -After=multi-user.target - -[Service] -Type=idle -ExecStart=/usr/bin/python /path/roslibpy_client.py - -[Install] -WantedBy=multi-user.target -``` -3. Reload the systemd manager configuration: `sudo systemctl daemon-reload`. -4. Enable the service: `sudo systemctl enable pstop-autostart.service`. -5. This service should now start on boot! -- You can reboot either by power cycling or with `sudo reboot`. -- You can also test it without rebooting with `sudo systemctl start pstop-autostart.service`. +1. Edit the service file `nano` or your text editor of choice and add: + + ``` + [Unit] + Description=Start protective stop on boot + After=multi-user.target + + [Service] + Type=idle + ExecStart=/usr/bin/python /path/roslibpy_client.py + + [Install] + WantedBy=multi-user.target + ``` + +1. Reload the systemd manager configuration: `sudo systemctl daemon-reload`. +1. Enable the service: `sudo systemctl enable pstop-autostart.service`. +1. This service should now start on boot! + - You can reboot either by power cycling or with `sudo reboot`. + - You can also test it without rebooting with `sudo systemctl start pstop-autostart.service`. diff --git a/archive/protective_stop_remote/protective_stop_remote/__init__.py b/archive/protective_stop_remote/protective_stop_remote/__init__.py index e69de29..18a2699 100644 --- a/archive/protective_stop_remote/protective_stop_remote/__init__.py +++ b/archive/protective_stop_remote/protective_stop_remote/__init__.py @@ -0,0 +1,2 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 diff --git a/archive/protective_stop_remote/protective_stop_remote/app.py b/archive/protective_stop_remote/protective_stop_remote/app.py index 34f9597..22d6b09 100644 --- a/archive/protective_stop_remote/protective_stop_remote/app.py +++ b/archive/protective_stop_remote/protective_stop_remote/app.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 # Handles configuration via webpage + saving config to json file import json diff --git a/archive/protective_stop_remote/protective_stop_remote/button.py b/archive/protective_stop_remote/protective_stop_remote/button.py index b68ecab..769b3d5 100644 --- a/archive/protective_stop_remote/protective_stop_remote/button.py +++ b/archive/protective_stop_remote/protective_stop_remote/button.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 # Handles pstop button physical interaction import logging diff --git a/archive/protective_stop_remote/protective_stop_remote/main.py b/archive/protective_stop_remote/protective_stop_remote/main.py index 26b91d9..940355a 100644 --- a/archive/protective_stop_remote/protective_stop_remote/main.py +++ b/archive/protective_stop_remote/protective_stop_remote/main.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 # Automatically started on boot, combines data from other nodes from multiprocessing import Manager, Process diff --git a/archive/protective_stop_remote/protective_stop_remote/power.py b/archive/protective_stop_remote/protective_stop_remote/power.py index 99f8e14..4e50efa 100644 --- a/archive/protective_stop_remote/protective_stop_remote/power.py +++ b/archive/protective_stop_remote/protective_stop_remote/power.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 # Handles power button, UPS management, shutdown command import logging diff --git a/archive/protective_stop_remote/protective_stop_remote/pstop.py b/archive/protective_stop_remote/protective_stop_remote/pstop.py index 8616a49..999150c 100644 --- a/archive/protective_stop_remote/protective_stop_remote/pstop.py +++ b/archive/protective_stop_remote/protective_stop_remote/pstop.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 import base64 import logging import threading diff --git a/archive/protective_stop_remote/protective_stop_remote/ui.py b/archive/protective_stop_remote/protective_stop_remote/ui.py index 154077f..0dffa94 100644 --- a/archive/protective_stop_remote/protective_stop_remote/ui.py +++ b/archive/protective_stop_remote/protective_stop_remote/ui.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 # Controls the LED ring, E paper display # Due to control of LED ring, must be run as root on raspi diff --git a/archive/protective_stop_remote/setup.py b/archive/protective_stop_remote/setup.py index 0c936d3..dbb80fa 100644 --- a/archive/protective_stop_remote/setup.py +++ b/archive/protective_stop_remote/setup.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 import os from glob import glob diff --git a/archive/protective_stop_remote/tests/__init__.py b/archive/protective_stop_remote/tests/__init__.py index e69de29..18a2699 100644 --- a/archive/protective_stop_remote/tests/__init__.py +++ b/archive/protective_stop_remote/tests/__init__.py @@ -0,0 +1,2 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 diff --git a/archive/protective_stop_remote/tests/test_app.py b/archive/protective_stop_remote/tests/test_app.py index f57ccff..a46ea3b 100644 --- a/archive/protective_stop_remote/tests/test_app.py +++ b/archive/protective_stop_remote/tests/test_app.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 import pytest from protective_stop_remote.app import config_app diff --git a/protective_stop_msg/CMakeLists.txt b/protective_stop_msg/CMakeLists.txt index 761e2ff..ddec753 100644 --- a/protective_stop_msg/CMakeLists.txt +++ b/protective_stop_msg/CMakeLists.txt @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 cmake_minimum_required(VERSION 3.8) project(protective_stop_msg) diff --git a/protective_stop_node/CMakeLists.txt b/protective_stop_node/CMakeLists.txt index 88480d3..eaffdcf 100644 --- a/protective_stop_node/CMakeLists.txt +++ b/protective_stop_node/CMakeLists.txt @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 cmake_minimum_required(VERSION 3.5) project(protective_stop_node) diff --git a/protective_stop_node/protective_stop_node/__init__.py b/protective_stop_node/protective_stop_node/__init__.py index e69de29..18a2699 100644 --- a/protective_stop_node/protective_stop_node/__init__.py +++ b/protective_stop_node/protective_stop_node/__init__.py @@ -0,0 +1,2 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 diff --git a/protective_stop_node/protective_stop_node/models.py b/protective_stop_node/protective_stop_node/models.py index 3f002a5..b8c8bab 100644 --- a/protective_stop_node/protective_stop_node/models.py +++ b/protective_stop_node/protective_stop_node/models.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 from enum import Enum from typing import Optional diff --git a/protective_stop_node/protective_stop_node/protective_stop_node.py b/protective_stop_node/protective_stop_node/protective_stop_node.py index fd26ed1..16fc619 100644 --- a/protective_stop_node/protective_stop_node/protective_stop_node.py +++ b/protective_stop_node/protective_stop_node/protective_stop_node.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 import threading import traceback from functools import partial diff --git a/protective_stop_node/protective_stop_node/test_utils/__init__.py b/protective_stop_node/protective_stop_node/test_utils/__init__.py index e69de29..18a2699 100644 --- a/protective_stop_node/protective_stop_node/test_utils/__init__.py +++ b/protective_stop_node/protective_stop_node/test_utils/__init__.py @@ -0,0 +1,2 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 diff --git a/protective_stop_node/protective_stop_node/test_utils/base_test.py b/protective_stop_node/protective_stop_node/test_utils/base_test.py index 97a7847..3632ac2 100644 --- a/protective_stop_node/protective_stop_node/test_utils/base_test.py +++ b/protective_stop_node/protective_stop_node/test_utils/base_test.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 import threading import time import unittest diff --git a/protective_stop_node/protective_stop_node/test_utils/helpers.py b/protective_stop_node/protective_stop_node/test_utils/helpers.py index 91e49c2..51f8d0d 100644 --- a/protective_stop_node/protective_stop_node/test_utils/helpers.py +++ b/protective_stop_node/protective_stop_node/test_utils/helpers.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 import time from builtin_interfaces.msg import Time diff --git a/protective_stop_node/test/launch.test.py b/protective_stop_node/test/launch.test.py index db27fb9..328e37c 100644 --- a/protective_stop_node/test/launch.test.py +++ b/protective_stop_node/test/launch.test.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 # !/usr/bin/env python3 import time import uuid diff --git a/protective_stop_node/test/unmonitored_mode.test.py b/protective_stop_node/test/unmonitored_mode.test.py index 8204b8a..d961378 100644 --- a/protective_stop_node/test/unmonitored_mode.test.py +++ b/protective_stop_node/test/unmonitored_mode.test.py @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 # !/usr/bin/env python3 import time diff --git a/pstop_c/CMakeLists.txt b/pstop_c/CMakeLists.txt index ac3bcc4..14eb931 100644 --- a/pstop_c/CMakeLists.txt +++ b/pstop_c/CMakeLists.txt @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 cmake_minimum_required(VERSION 3.12) project(PSTOP C) diff --git a/pstop_c/pstop/CMakeLists.txt b/pstop_c/pstop/CMakeLists.txt index 094de84..bc854e6 100644 --- a/pstop_c/pstop/CMakeLists.txt +++ b/pstop_c/pstop/CMakeLists.txt @@ -1,3 +1,5 @@ +# SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +# SPDX-License-Identifier: Apache-2.0 cmake_minimum_required(VERSION 3.12) project(PSTOPCore C) diff --git a/pstop_c/pstop/include/pstop/checksum.h b/pstop_c/pstop/include/pstop/checksum.h index ab15794..6e1c3ca 100644 --- a/pstop_c/pstop/include/pstop/checksum.h +++ b/pstop_c/pstop/include/pstop/checksum.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_CHECKSUM_H #define PSTOP_CHECKSUM_H diff --git a/pstop_c/pstop/include/pstop/constants.h b/pstop_c/pstop/include/pstop/constants.h index 7f92c43..1ffba91 100644 --- a/pstop_c/pstop/include/pstop/constants.h +++ b/pstop_c/pstop/include/pstop/constants.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_CONSTANTS_H #define PSTOP_CONSTANTS_H diff --git a/pstop_c/pstop/include/pstop/device_id.h b/pstop_c/pstop/include/pstop/device_id.h index 65598f2..69bb7ce 100644 --- a/pstop_c/pstop/include/pstop/device_id.h +++ b/pstop_c/pstop/include/pstop/device_id.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_DEVICE_ID_H #define PSTOP_DEVICE_ID_H diff --git a/pstop_c/pstop/include/pstop/error.h b/pstop_c/pstop/include/pstop/error.h index f90f478..62db677 100644 --- a/pstop_c/pstop/include/pstop/error.h +++ b/pstop_c/pstop/include/pstop/error.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_ERROR_H #define PSTOP_ERROR_H diff --git a/pstop_c/pstop/include/pstop/machine.h b/pstop_c/pstop/include/pstop/machine.h index 60062f7..276256c 100644 --- a/pstop_c/pstop/include/pstop/machine.h +++ b/pstop_c/pstop/include/pstop/machine.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_MACHINE_H #define PSTOP_MACHINE_H diff --git a/pstop_c/pstop/include/pstop/protocol.h b/pstop_c/pstop/include/pstop/protocol.h index 01d42bc..783dcf2 100644 --- a/pstop_c/pstop/include/pstop/protocol.h +++ b/pstop_c/pstop/include/pstop/protocol.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_PROTOCOL_H #define PSTOP_PROTOCOL_H diff --git a/pstop_c/pstop/include/pstop/pstop_application.h b/pstop_c/pstop/include/pstop/pstop_application.h index ad463ca..81ac1d4 100644 --- a/pstop_c/pstop/include/pstop/pstop_application.h +++ b/pstop_c/pstop/include/pstop/pstop_application.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_PSTOP_APPLICATION_H #define PSTOP_PSTOP_APPLICATION_H diff --git a/pstop_c/pstop/include/pstop/pstop_client.h b/pstop_c/pstop/include/pstop/pstop_client.h index 04ccf00..96df13f 100644 --- a/pstop_c/pstop/include/pstop/pstop_client.h +++ b/pstop_c/pstop/include/pstop/pstop_client.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_PSTOP_CLIENT_H #define PSTOP_PSTOP_CLIENT_H diff --git a/pstop_c/pstop/include/pstop/pstop_msg.h b/pstop_c/pstop/include/pstop/pstop_msg.h index 64c2ca7..7deee46 100644 --- a/pstop_c/pstop/include/pstop/pstop_msg.h +++ b/pstop_c/pstop/include/pstop/pstop_msg.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_PSTOP_MSG_H #define PSTOP_PSTOP_MSG_H diff --git a/pstop_c/pstop/include/pstop/time.h b/pstop_c/pstop/include/pstop/time.h index bd03723..842c852 100644 --- a/pstop_c/pstop/include/pstop/time.h +++ b/pstop_c/pstop/include/pstop/time.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_TIME_H #define PSTOP_TIME_H diff --git a/pstop_c/pstop/src/pstop/checksum.c b/pstop_c/pstop/src/pstop/checksum.c index 2fa8e49..3faeac9 100644 --- a/pstop_c/pstop/src/pstop/checksum.c +++ b/pstop_c/pstop/src/pstop/checksum.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include #include "pstop/checksum.h" diff --git a/pstop_c/pstop/src/pstop/device_id.c b/pstop_c/pstop/src/pstop/device_id.c index 2ab5e8d..3a231c8 100644 --- a/pstop_c/pstop/src/pstop/device_id.c +++ b/pstop_c/pstop/src/pstop/device_id.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include #include "pstop/device_id.h" diff --git a/pstop_c/pstop/src/pstop/machine.c b/pstop_c/pstop/src/pstop/machine.c index 6034f6e..59f735b 100644 --- a/pstop_c/pstop/src/pstop/machine.c +++ b/pstop_c/pstop/src/pstop/machine.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include #include diff --git a/pstop_c/pstop/src/pstop/protocol.c b/pstop_c/pstop/src/pstop/protocol.c index 20b15e9..ba81123 100644 --- a/pstop_c/pstop/src/pstop/protocol.c +++ b/pstop_c/pstop/src/pstop/protocol.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include #include "pstop/protocol.h" diff --git a/pstop_c/pstop/src/pstop/pstop_application.c b/pstop_c/pstop/src/pstop/pstop_application.c index 3a5a3a5..9274e2a 100644 --- a/pstop_c/pstop/src/pstop/pstop_application.c +++ b/pstop_c/pstop/src/pstop/pstop_application.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include #include "pstop/pstop_application.h" diff --git a/pstop_c/pstop/src/pstop/pstop_client.c b/pstop_c/pstop/src/pstop/pstop_client.c index cf69bba..b2f928d 100644 --- a/pstop_c/pstop/src/pstop/pstop_client.c +++ b/pstop_c/pstop/src/pstop/pstop_client.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include #include diff --git a/pstop_c/pstop/src/pstop/pstop_msg.c b/pstop_c/pstop/src/pstop/pstop_msg.c index 7225ace..45a87b9 100644 --- a/pstop_c/pstop/src/pstop/pstop_msg.c +++ b/pstop_c/pstop/src/pstop/pstop_msg.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include "pstop/pstop_msg.h" #include "pstop/checksum.h" #include "pstop/constants.h" diff --git a/pstop_c/pstop/src/pstop/time.c b/pstop_c/pstop/src/pstop/time.c index 4c4936b..ff66e41 100644 --- a/pstop_c/pstop/src/pstop/time.c +++ b/pstop_c/pstop/src/pstop/time.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include #include "pstop/time.h" diff --git a/pstop_c/pstop/test/include/pstop/test_utils.h b/pstop_c/pstop/test/include/pstop/test_utils.h index 322f99e..9497a1c 100644 --- a/pstop_c/pstop/test/include/pstop/test_utils.h +++ b/pstop_c/pstop/test/include/pstop/test_utils.h @@ -1,3 +1,6 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #ifndef PSTOP_TEST_UTILS_H #define PSTOP_TEST_UTILS_H diff --git a/pstop_c/pstop/test/src/pstop/device_id_test.c b/pstop_c/pstop/test/src/pstop/device_id_test.c index 757c0b0..d7eb487 100644 --- a/pstop_c/pstop/test/src/pstop/device_id_test.c +++ b/pstop_c/pstop/test/src/pstop/device_id_test.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include "pstop/device_id.h" #include diff --git a/pstop_c/pstop/test/src/pstop/machine_test.c b/pstop_c/pstop/test/src/pstop/machine_test.c index 4451bde..6aa16e0 100644 --- a/pstop_c/pstop/test/src/pstop/machine_test.c +++ b/pstop_c/pstop/test/src/pstop/machine_test.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include "pstop/machine.h" #include diff --git a/pstop_c/pstop/test/src/pstop/machine_timeout_test.c b/pstop_c/pstop/test/src/pstop/machine_timeout_test.c index 7dd45cf..a8dd531 100644 --- a/pstop_c/pstop/test/src/pstop/machine_timeout_test.c +++ b/pstop_c/pstop/test/src/pstop/machine_timeout_test.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include "pstop/machine.h" #include diff --git a/pstop_c/pstop/test/src/pstop/main.c b/pstop_c/pstop/test/src/pstop/main.c index a959dc0..88fc424 100644 --- a/pstop_c/pstop/test/src/pstop/main.c +++ b/pstop_c/pstop/test/src/pstop/main.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include extern void main_device_id_test(void); diff --git a/pstop_c/pstop/test/src/pstop/pstop_client_test.c b/pstop_c/pstop/test/src/pstop/pstop_client_test.c index 0a4e8c4..60e17b9 100644 --- a/pstop_c/pstop/test/src/pstop/pstop_client_test.c +++ b/pstop_c/pstop/test/src/pstop/pstop_client_test.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include "pstop/pstop_client.h" #include diff --git a/pstop_c/pstop/test/src/pstop/test_utils.c b/pstop_c/pstop/test/src/pstop/test_utils.c index 3843bdb..d564953 100644 --- a/pstop_c/pstop/test/src/pstop/test_utils.c +++ b/pstop_c/pstop/test/src/pstop/test_utils.c @@ -1,4 +1,7 @@ +// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. +// SPDX-License-Identifier: Apache-2.0 + #include #include "pstop/test_utils.h" From 5dd39fda8edd8bb2cc5e3d4b395b24827131f839 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 28 Apr 2026 16:41:36 -0700 Subject: [PATCH 3/5] Add pre-commit ci also --- .github/workflows/pre-commit.yml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 .github/workflows/pre-commit.yml diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 0000000..3ec904e --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,17 @@ +--- +name: Pre-commit + +on: + push: + branches: [main] + pull_request: + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v6 + - uses: actions/setup-python@v6 + with: + python-version: '3.10' + - uses: pre-commit/action@v3.0.1 From 55f0bd98b230ea04ac264f88a0a1db1c4a45e464 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 28 Apr 2026 16:52:45 -0700 Subject: [PATCH 4/5] Use a proper tag, not local checkout --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 89b9f90..3923d8a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,8 +1,8 @@ --- # Apply Polymath Code Standard formatters and linters repos: - - repo: ~/dev/polymath/infra/polymath_code_standard - rev: feafa20be0c307cccb867209885ce4f83a34e9ce + - repo: https://github.com/polymathrobotics/polymath_code_standard + rev: v2.1.1 hooks: # Everything - id: polymath-general From 6eb28a8437588feb7c7d82bd87a27e65eeb61fa2 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 28 Apr 2026 16:53:51 -0700 Subject: [PATCH 5/5] No failfast ros2 matrix --- .github/workflows/build.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 93855d8..cf27180 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -10,6 +10,7 @@ jobs: build_and_test_ros2: runs-on: ubuntu-latest strategy: + fail-fast: false matrix: include: - rosdistro: humble