mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-04-20 03:41:17 -05:00
tested it on testbed and had to change things
This commit is contained in:
@@ -86,7 +86,7 @@ class Anchor(Node):
|
|||||||
try:
|
try:
|
||||||
logger.info("trying CAN connector")
|
logger.info("trying CAN connector")
|
||||||
self.connector = CANConnector(self.get_logger())
|
self.connector = CANConnector(self.get_logger())
|
||||||
except (NoValidDeviceException, NoWorkingDeviceException):
|
except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
|
||||||
logger.info("CAN connector failed, trying serial connector")
|
logger.info("CAN connector failed, trying serial connector")
|
||||||
self.connector = SerialConnector(self.get_logger())
|
self.connector = SerialConnector(self.get_logger())
|
||||||
case _:
|
case _:
|
||||||
|
|||||||
@@ -63,19 +63,25 @@ class SerialConnector(Connector):
|
|||||||
self.logger.info(f"asking {port} for its name")
|
self.logger.info(f"asking {port} for its name")
|
||||||
serial_interface = serial.Serial(port, BAUD_RATE, timeout=1)
|
serial_interface = serial.Serial(port, BAUD_RATE, timeout=1)
|
||||||
|
|
||||||
|
serial_interface.write(
|
||||||
|
b"can_relay_mode,on\n"
|
||||||
|
)
|
||||||
|
|
||||||
for i in range(4):
|
for i in range(4):
|
||||||
self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name")
|
self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name")
|
||||||
response = serial_interface.read_until(bytes("\n", "utf8"))
|
for _ in range(4):
|
||||||
try:
|
response = serial_interface.read_until(bytes("\n", "utf8"))
|
||||||
if b"can_relay_ready" in response:
|
try:
|
||||||
args = response.decode("utf8").strip().split(",")
|
if b"can_relay_ready" in response:
|
||||||
if len(args) == 2:
|
args: list[str] = response.decode("utf8").strip().split(",")
|
||||||
return args[1]
|
if len(args) == 2:
|
||||||
break
|
self.logger.info(f"we are talking to {args[1]}")
|
||||||
except UnicodeDecodeError as e:
|
return args[1]
|
||||||
self.logger.debug(
|
break
|
||||||
f"ignoring UnicodeDecodeError when asking for MCU name: {e}"
|
except UnicodeDecodeError as e:
|
||||||
)
|
self.logger.info(
|
||||||
|
f"ignoring UnicodeDecodeError when asking for MCU name: {e}"
|
||||||
|
)
|
||||||
|
|
||||||
if serial_interface.is_open:
|
if serial_interface.is_open:
|
||||||
serial_interface.close()
|
serial_interface.close()
|
||||||
@@ -95,13 +101,13 @@ class SerialConnector(Connector):
|
|||||||
map( # get just device strings
|
map( # get just device strings
|
||||||
lambda p: p.device,
|
lambda p: p.device,
|
||||||
filter( # make sure we have a known device
|
filter( # make sure we have a known device
|
||||||
lambda p: (p.vid, p.pid, p.device) in KNOWN_USBS
|
lambda p: (p.vid, p.pid) in KNOWN_USBS
|
||||||
and p.device is not None,
|
and p.device is not None,
|
||||||
comports,
|
comports,
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
self.logger.debug(f"found valid MCU ports: [ {', '.join(valid_ports)} ]")
|
self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]")
|
||||||
return valid_ports
|
return valid_ports
|
||||||
|
|
||||||
def cleanup(self):
|
def cleanup(self):
|
||||||
@@ -126,7 +132,7 @@ class SerialConnector(Connector):
|
|||||||
|
|
||||||
# check each of our ports to make sure one of them is responding
|
# check each of our ports to make sure one of them is responding
|
||||||
port = ports[0]
|
port = ports[0]
|
||||||
mcu_name = "mock" if self.override else self._get_name(port)
|
mcu_name = self._get_name(port)
|
||||||
if not mcu_name:
|
if not mcu_name:
|
||||||
raise NoWorkingDeviceException(
|
raise NoWorkingDeviceException(
|
||||||
f"found {port}, but it did not respond with its name"
|
f"found {port}, but it did not respond with its name"
|
||||||
|
|||||||
@@ -6,15 +6,15 @@ def string_to_viccan(msg: str, mcu_name, logger: RcutilsLogger):
|
|||||||
|
|
||||||
# don't need an extra check because len of .split output is always >= 1
|
# don't need an extra check because len of .split output is always >= 1
|
||||||
if parts[0] != "can_relay_fromvic":
|
if parts[0] != "can_relay_fromvic":
|
||||||
logger.info(f"got non-CAN data from {mcu_name}: {msg}")
|
logger.debug(f"got non-CAN data from {mcu_name}: {msg}")
|
||||||
return None
|
return None
|
||||||
elif len(parts) < 3:
|
elif len(parts) < 3:
|
||||||
logger.info(
|
logger.debug(
|
||||||
f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}"
|
f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}"
|
||||||
)
|
)
|
||||||
return None
|
return None
|
||||||
elif len(parts) > 7:
|
elif len(parts) > 7:
|
||||||
logger.info(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}")
|
logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}")
|
||||||
return None
|
return None
|
||||||
|
|
||||||
return VicCAN(
|
return VicCAN(
|
||||||
|
|||||||
Reference in New Issue
Block a user