mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-02-11 09:20:40 +00:00
feat: (anchor) add new Serial finder code
Uses vendor and product ids to find a microcontroller, and detects its name after connecting. Upon failure, falls back to Areeb's code--just in case. Also renamed `self.ser` to `self.serial_interface` and `self.port` to `self.serial_port` for clarity.
This commit is contained in:
David
@@ -8,6 +8,7 @@ import time
|
||||
import atexit
|
||||
|
||||
import serial
|
||||
import serial.tools.list_ports
|
||||
import os
|
||||
import sys
|
||||
import threading
|
||||
@@ -16,6 +17,13 @@ import glob
|
||||
from std_msgs.msg import String, Header
|
||||
from astra_msgs.msg import VicCAN
|
||||
|
||||
KNOWN_USBS = [
|
||||
(0x2E8A, 0x00C0), # Raspberry Pi Pico
|
||||
(0x1A86, 0x55D4), # Adafruit Feather ESP32 V2
|
||||
(0x10C4, 0xEA60), # DOIT ESP32 Devkit V1
|
||||
(0x1A86, 0x55D3), # ESP32 S3 Development Board
|
||||
]
|
||||
|
||||
|
||||
class Anchor(Node):
|
||||
"""
|
||||
@@ -42,13 +50,61 @@ class Anchor(Node):
|
||||
# Initalize node with name
|
||||
super().__init__("anchor_node") # previously 'serial_publisher'
|
||||
|
||||
# Loop through all serial devices on the computer to check for the MCU
|
||||
self.port = None
|
||||
self.serial_port: str | None = None # e.g., "/dev/ttyUSB0"
|
||||
|
||||
# Serial port override
|
||||
if port_override := os.getenv("PORT_OVERRIDE"):
|
||||
self.port = port_override
|
||||
self.serial_port = port_override
|
||||
|
||||
##################################################
|
||||
# Serial MCU Discovery
|
||||
|
||||
# If there was not a port override, look for a MCU over USB for Serial.
|
||||
if self.serial_port is None:
|
||||
comports = serial.tools.list_ports.comports()
|
||||
real_ports = list(
|
||||
filter(
|
||||
lambda p: p.vid is not None
|
||||
and p.pid is not None
|
||||
and p.device is not None,
|
||||
comports,
|
||||
)
|
||||
)
|
||||
recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
|
||||
|
||||
if len(recog_ports) == 1: # Found singular recognized MCU
|
||||
found_port = recog_ports[0]
|
||||
self.get_logger().info(
|
||||
f"Selecting MCU '{found_port.description}' at {found_port.device}."
|
||||
)
|
||||
self.serial_port = found_port.device
|
||||
elif len(recog_ports) > 1: # Found multiple recognized MCUs
|
||||
# Kinda jank log message
|
||||
self.get_logger().error(
|
||||
f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
|
||||
)
|
||||
# time.sleep(1)
|
||||
# sys.exit(1)
|
||||
elif (
|
||||
len(recog_ports) == 0 and len(real_ports) > 0
|
||||
): # Found real ports but none recognized
|
||||
self.get_logger().error(
|
||||
f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
|
||||
)
|
||||
# time.sleep(1)
|
||||
# sys.exit(1)
|
||||
else: # Found jack shit
|
||||
self.get_logger().error("No valid Serial ports specified or found.")
|
||||
# time.sleep(1)
|
||||
# sys.exit(1)
|
||||
|
||||
# We still don't have a serial port; fall back to legacy discovery (Areeb's code)
|
||||
# Loop through all serial devices on the computer to check for the MCU
|
||||
if self.serial_port is None:
|
||||
self.get_logger().warning("Falling back to legacy MCU discovery...")
|
||||
ports = Anchor.list_serial_ports()
|
||||
for i in range(4):
|
||||
if self.port is not None:
|
||||
for _ in range(4):
|
||||
if self.serial_port is not None:
|
||||
break
|
||||
for port in ports:
|
||||
try:
|
||||
@@ -60,20 +116,50 @@ class Anchor(Node):
|
||||
|
||||
# if pong is in response, then we are talking with the MCU
|
||||
if b"pong" in response:
|
||||
self.port = port
|
||||
self.get_logger().info(f"Found MCU at {self.port}!")
|
||||
self.serial_port = port
|
||||
self.get_logger().info(f"Found MCU at {self.serial_port}!")
|
||||
break
|
||||
except:
|
||||
pass
|
||||
|
||||
if self.port is None:
|
||||
self.get_logger().info("Unable to find MCU...")
|
||||
# If port is still None then we ain't finding no mcu
|
||||
if self.serial_port is None:
|
||||
self.get_logger().error("Unable to find MCU. Exiting...")
|
||||
time.sleep(1)
|
||||
sys.exit(1)
|
||||
# Found a Serial port, try to open it; above code has not officially opened a Serial port
|
||||
else:
|
||||
self.get_logger().debug(
|
||||
f"Attempting to open Serial port '{self.serial_port}'..."
|
||||
)
|
||||
try:
|
||||
self.serial_interface = serial.Serial(
|
||||
self.serial_port, 115200, timeout=1
|
||||
)
|
||||
|
||||
self.ser = serial.Serial(self.port, 115200)
|
||||
self.get_logger().info(f"Enabling Relay Mode")
|
||||
self.ser.write(b"can_relay_mode,on\n")
|
||||
# Attempt to get name of connected MCU
|
||||
self.serial_interface.write(
|
||||
b"can_relay_mode,on\n"
|
||||
) # can_relay_ready,[mcu]
|
||||
mcu_name: str = ""
|
||||
for _ in range(4):
|
||||
response = self.serial_interface.read_until(bytes("\n", "utf8"))
|
||||
if b"can_relay_ready" in response:
|
||||
args: list[str] = response.decode("utf8").strip().split(",")
|
||||
if len(args) == 2:
|
||||
mcu_name = args[1]
|
||||
break
|
||||
self.get_logger().info(
|
||||
f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
|
||||
)
|
||||
|
||||
except serial.SerialException as e:
|
||||
self.get_logger().error(
|
||||
f"Could not open Serial port '{self.serial_port}' for reason:"
|
||||
)
|
||||
self.get_logger().error(e.strerror)
|
||||
time.sleep(1)
|
||||
sys.exit(1)
|
||||
|
||||
# Close serial port on exit
|
||||
atexit.register(self.cleanup)
|
||||
@@ -120,7 +206,7 @@ class Anchor(Node):
|
||||
def read_MCU(self):
|
||||
"""Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
|
||||
try:
|
||||
output = str(self.ser.readline(), "utf8")
|
||||
output = str(self.serial_interface.readline(), "utf8")
|
||||
|
||||
if output:
|
||||
self.relay_fromvic(output)
|
||||
@@ -146,14 +232,20 @@ class Anchor(Node):
|
||||
except serial.SerialException as e:
|
||||
print(f"SerialException: {e}")
|
||||
print("Closing serial port.")
|
||||
if self.ser.is_open:
|
||||
self.ser.close()
|
||||
try:
|
||||
if self.serial_interface.is_open:
|
||||
self.serial_interface.close()
|
||||
except:
|
||||
pass
|
||||
exit(1)
|
||||
except TypeError as e:
|
||||
print(f"TypeError: {e}")
|
||||
print("Closing serial port.")
|
||||
if self.ser.is_open:
|
||||
self.ser.close()
|
||||
try:
|
||||
if self.serial_interface.is_open:
|
||||
self.serial_interface.close()
|
||||
except:
|
||||
pass
|
||||
exit(1)
|
||||
except Exception as e:
|
||||
print(f"Exception: {e}")
|
||||
@@ -174,7 +266,7 @@ class Anchor(Node):
|
||||
output += f",{round(num, 7)}" # limit to 7 decimal places
|
||||
output += "\n"
|
||||
# self.get_logger().info(f"VicCAN relay to MCU: {output}")
|
||||
self.ser.write(bytes(output, "utf8"))
|
||||
self.serial_interface.write(bytes(output, "utf8"))
|
||||
|
||||
def relay_fromvic(self, msg: str):
|
||||
"""Relay a string message from the MCU to the appropriate VicCAN topic"""
|
||||
@@ -236,7 +328,7 @@ class Anchor(Node):
|
||||
"""Relay a raw string message to the MCU for debugging"""
|
||||
message = msg.data
|
||||
# self.get_logger().info(f"Sending command to MCU: {msg}")
|
||||
self.ser.write(bytes(message, "utf8"))
|
||||
self.serial_interface.write(bytes(message, "utf8"))
|
||||
|
||||
@staticmethod
|
||||
def list_serial_ports():
|
||||
@@ -244,8 +336,8 @@ class Anchor(Node):
|
||||
|
||||
def cleanup(self):
|
||||
print("Cleaning up before terminating...")
|
||||
if self.ser.is_open:
|
||||
self.ser.close()
|
||||
if self.serial_interface.is_open:
|
||||
self.serial_interface.close()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
|
||||
Reference in New Issue
Block a user