feat: replay system for can (#174)

* added ability to store and replay can messages

* added some ease of use features to the replay of the can messages.

- added checks if replay file exists.
- only allow recording when driving with controller
- only listen to the CANContolIdentifiers instead of all messages
- moved toggle recording to the manual mode
- made some changes to replay script to make it more robust and easier to use

* small improvements to cli arguments

* ruff format

* moved can recording to a script and updated braking_calibration to up-to-date conventions

* removed unused vars

* resolved comments

.gitignore

@@ -21,7 +21,7 @@ venv.bak/
 .vscode
 
 # Data
-**/.benchmarks
+**/can_recordings
 
 # Compiled object detection model
 *_openvino_model/

.ruff.toml

@@ -52,7 +52,6 @@ exclude = ["scripts/python/original_controller.py"]
 # Ignore unused imports in __init__.py files (would need __all__ otherwise)
 [lint.per-file-ignores]
 "__init__.py" = ["F401", "I"]
-"scripts/python/braking_calibration.py" = ["ARG002", "ANN003", "ANN002"]
 
 [format]
 quote-style = "double"

scripts/python/braking_calibration.py

@@ -1,9 +1,9 @@
-import can
 import logging
+import time
 
-from os import system
+from typing import Any
 
-from src.driving.can import CANController
+from src.driving.can import CANController, get_can_bus
 from src.driving.gamepad import EventType, Gamepad, GamepadAxis, GamepadButton
 from src.driving.modes import ManualDriving
 
@@ -55,19 +55,18 @@ def __init__(self, can_controller: CANController, gamepad: Gamepad) -> None:
         self.low_idx = 0
         self.high_idx = len(self.braking_steps) - 1
         self.middle_idx = self.low_idx + (self.high_idx - self.low_idx) // 2
-        self.logger = logging.getLogger(__name__)
 
-    def start_procedure(self, *args, **kwargs) -> None:
+    def start_procedure(self, *_args: Any, **_kwargs: Any) -> None:
         """Start the brake calibration procedure.
 
         This will register all needed callbacks. Next to this it often won't do
         anything else. This function should be called when the B button is pressed.
         """
         if self.started:
-            self.logger.warning("brake calibration already started")
+            logging.warning("brake calibration already started")
             return
 
-        self.logger.info("start brake calibration")
+        logging.info("start brake calibration")
         self.gamepad.add_listener(GamepadAxis.DPAD_X, EventType.AXIS_CHANGED, self.__arrow_pressed)
         self.gamepad.add_listener(GamepadAxis.DPAD_Y, EventType.AXIS_CHANGED, self.__arrow_pressed)
         self.gamepad.add_listener(GamepadButton.LB, EventType.BUTTON_DOWN, self.__start_braking)
@@ -86,13 +85,13 @@ def __arrow_pressed(self, button: GamepadAxis, _event: EventType, val: float) ->
 
         if button == GamepadAxis.DPAD_Y:
             if val == 1:
-                self.logger.info("select lockup")
+                logging.info("select lockup")
                 self.locked = True
             elif val == -1:
-                self.logger.info("select non lockup")
+                logging.info("select non lockup")
                 self.locked = False
         if button == GamepadAxis.DPAD_X:
-            self.logger.info("confirm")
+            logging.info("confirm")
             if val == 1:
                 self.__confirm_lockup()
 
@@ -105,18 +104,18 @@ def __confirm_lockup(self) -> None:
         """
         # check if we have braked.
         if not self.braked:
-            self.logger.info("brake first")
+            logging.info("brake first")
             return
 
         self.braked = False
 
         with open("braking_force.txt", "w") as f:
             if self.low_idx >= self.high_idx:
                 f.write(f"braking force calibrated: {self.braking_steps[self.middle_idx]}")
-                self.logger.info("braking force calibrated: %d", self.braking_steps[self.middle_idx])
+                logging.info("braking force calibrated: %d", self.braking_steps[self.middle_idx])
 
             f.write(f"low: {self.low_idx} high: {self.high_idx} middle: {self.middle_idx}")
-            self.logger.info("low: %d high: %d middle: %d", self.low_idx, self.high_idx, self.middle_idx)
+            logging.info("low: %d high: %d middle: %d", self.low_idx, self.high_idx, self.middle_idx)
             if self.locked:
                 self.high_idx = self.middle_idx - 1
             else:
@@ -126,22 +125,20 @@ def __confirm_lockup(self) -> None:
         self.locked = False
         self.middle_idx = self.low_idx + (self.high_idx - self.low_idx) // 2
 
-    def __start_braking(self, *args, **kwargs) -> None:
+    def __start_braking(self, *_args: Any, **_kwargs: Any) -> None:
         """Start braking at the set pressure."""
         self.can_controller.set_brake(self.braking_steps[self.middle_idx])
 
-    def __stop_braking(self, *args, **kwargs) -> None:
+    def __stop_braking(self, *_args: Any, **_kwargs: Any) -> None:
         """Stop braking."""
         self.can_controller.set_brake(0)
         self.braked = True
 
 
 if __name__ == "__main__":
-    logging.basicConfig(level=logging.DEBUG)
-    system("ip link set can0 type can bitrate 500000")
-    system("ip link set can0 up")
+    logging.basicConfig(level=logging.INFO)
 
-    can_bus = can.interface.Bus(interface="socketcan", channel="can0", bitrate=500000)
+    can_bus = get_can_bus()
     can_controller = CANController(can_bus)
     can_controller.start()
 
@@ -154,4 +151,4 @@ def __stop_braking(self, *args, **kwargs) -> None:
     brake_calibration = BrakeCalibrationProcedure(can_controller, gamepad)
 
     while True:
-        pass
+        time.sleep(1)

scripts/python/can_recorder.py

@@ -0,0 +1,94 @@
+import can
+import logging
+import threading
+import time
+
+from datetime import datetime
+from pathlib import Path
+from typing import Any, Callable
+
+from src.constants import CANControlIdentifier
+from src.driving.can import CANController, get_can_bus
+from src.driving.gamepad import EventType, Gamepad, GamepadButton
+from src.driving.modes import ManualDriving
+
+
+class CANRecorder:
+    """Procedure to record CAN message.
+
+    This procedure can be used to record messages send on the CAN bus.
+    This will record all the messages we can send and not the ones we receive.
+    """
+
+    __can_bus: can.Bus
+    __recording: bool = False
+    __recorder: threading.Thread = None
+
+    def __init__(self) -> None:
+        """Procedure to record CAN messages."""
+        self.__can_bus = get_can_bus()
+        self.__can_bus.set_filters(
+            [{"can_id": can_id, "can_mask": 0xFFF, "extended": False} for can_id in CANControlIdentifier]
+        )
+
+    def toggle_recording(self) -> None:
+        """Toggle the recording of CAN messages.
+
+        This function will start recording CAN messages if it is not already recording,
+        and stop recording if it is already recording.
+        """
+        if not self.__recording:
+            path = Path(f"./data/can_recordings/{datetime.now().strftime("%m_%d_%Y_%H_%M_%S")}.asc")
+            path.parent.mkdir(parents=True, exist_ok=True)
+
+            logging.info("Recording CAN messages to %s", path)
+            self.__recorder = threading.Thread(target=self.__recording_thread, args=(path,), daemon=True)
+            self.__recorder.start()
+        else:
+            self.__recording = False
+            self.__recorder.join(1)
+
+            self.__can_bus.shutdown()
+            self.__can_bus = None
+
+            logging.info("Stopped recording CAN messages")
+
+    def __recording_thread(self, filepath: Path) -> None:
+        """Record CAN messages into a .asc file."""
+        self.__recording = True
+
+        with can.ASCWriter(filepath) as writer:
+            while self.__recording:
+                msg = self.__can_bus.recv(1)
+                if msg is not None:
+                    writer.on_message_received(msg)
+
+
+def create_toggle_callback(can_recorder: CANRecorder, gamepad: Gamepad) -> Callable[[Any, Any], None]:
+    """Create the callback for the toggle of the CAN recording."""
+
+    def __toggle(*_args: Any, **_kwargs: Any) -> None:
+        can_recorder.toggle_recording()
+        gamepad.vibrate()
+
+    return __toggle
+
+
+if __name__ == "__main__":
+    logging.basicConfig(level=logging.INFO)
+
+    can_bus = get_can_bus()
+    can_controller = CANController(can_bus)
+    can_controller.start()
+
+    gamepad = Gamepad()
+    gamepad.start()
+
+    controller_driving = ManualDriving(gamepad, can_controller)
+    controller_driving.start()
+
+    can_recorder = CANRecorder()
+    gamepad.add_listener(GamepadButton.LB, EventType.LONG_PRESS, create_toggle_callback(can_recorder, gamepad))
+
+    while True:
+        time.sleep(1)

scripts/python/can_replay.py

@@ -0,0 +1,92 @@
+import argparse
+import logging
+import threading
+
+from pathlib import Path
+
+from src.constants import CANControlIdentifier, CANFeedbackIdentifier
+from src.driving.can import get_can_bus, replay_log
+
+
+def log_all_can_messages(stopped: threading.Event, blacklist: list[int], whitelist: list[int]) -> threading.Thread:
+    """Log all messages of the CAN bus."""
+    listen_can = get_can_bus()
+
+    # create and apply filters to listen to only the messages we want to log
+    allowed_ids = set([int(v) for v in CANControlIdentifier] + [int(v) for v in CANFeedbackIdentifier]) - set(blacklist)
+
+    if len(whitelist) > 0:
+        allowed_ids = set(whitelist)
+
+    logging.info("Listening to messages with IDs: %s", [hex(i) for i in allowed_ids])
+    filters = [{"can_id": can_id, "can_mask": 0xFFF, "extended": False} for can_id in allowed_ids]
+    listen_can.set_filters(filters)
+
+    def __listen() -> None:
+        """Listen to the CAN bus and log all messages."""
+        while not stopped.is_set():
+            message = listen_can.recv()
+            print(message)  # noqa: T201
+
+        listen_can.shutdown()
+
+    thread = threading.Thread(target=__listen, daemon=True)
+    thread.start()
+    return thread
+
+
+if __name__ == "__main__":
+    """Replay CAN messages from a file and log to a file.
+
+    This script will replay CAN messages from a file and log them to a file.
+    We will also log them to the console depending on the arguments.
+    You can only use either --blacklist or --whitelist.
+
+    Arguments:
+        logfile: The path to the log file.
+        --log: Log all CAN messages.
+        --blacklist: The list of message IDs to ignore when logging. The IDs are in hexadecimal.
+        --whitelist: The list of message IDs to listen to. The IDs are in hexadecimal.
+    """
+    logging.basicConfig(level=logging.INFO)
+
+    arg_parser = argparse.ArgumentParser(description="Replay CAN messages from a file.")
+    arg_parser.add_argument("logfile", type=Path, help="The path to the log file.")
+    id_list_group = arg_parser.add_mutually_exclusive_group()
+    id_list_group.add_argument(
+        "--blacklist",
+        nargs="+",
+        type=lambda x: int(x, 0),
+        help="The list of message IDs to ignore when logging.",
+        default=[],
+    )
+    id_list_group.add_argument(
+        "--whitelist", nargs="+", type=lambda x: int(x, 0), help="The list of message IDs to listen to.", default=[]
+    )
+    id_list_group.add_argument("--log", action="store_true", help="Log all CAN messages.")
+    args = arg_parser.parse_args()
+
+    # Check if the log file exists.
+    if not args.logfile.exists():
+        logging.error("The log file %s does not exist.", args.logfile)
+        exit(1)
+
+    can_bus = get_can_bus()
+
+    # Setup and start the log thread if needed. If we want to log then we will set stop_event, so we can use stop_event
+    # not being None to know if we want to log or not.
+    log_thread = None
+    stop_event = threading.Event() if args.log or len(args.blacklist) > 0 or len(args.whitelist) > 0 else None
+    if stop_event is not None:
+        log_thread = log_all_can_messages(stop_event, args.blacklist, args.whitelist)
+
+    # Replay the log file.
+    logging.info("Replaying CAN messages from file...")
+    replay_log(can_bus, args.logfile)
+    logging.info("Replaying finished.")
+
+    if stop_event is not None:
+        stop_event.set()
+        log_thread.join(1)
+
+    can_bus.shutdown()

src/driving/can/__init__.py

@@ -1,3 +1,4 @@
 from .can_controller_interface import ICANController
 from .can_bus import get_can_bus
+from .can_bus import replay_log
 from .can_controller import CANController

src/driving/can/can_bus.py

@@ -2,6 +2,8 @@
 import logging
 import os
 
+from pathlib import Path
+
 
 def get_can_bus(channel: str = "can0", bitrate: int = 500000) -> can.ThreadSafeBus:
     """Create a CAN bus using the specified channel and bitrate.
@@ -20,3 +22,17 @@ def get_can_bus(channel: str = "can0", bitrate: int = 500000) -> can.ThreadSafeB
 
     logging.warning("Failed to create CAN interface, using virtual interface instead.")
     return can.ThreadSafeBus(interface="virtual", channel="vcan0")
+
+
+def replay_log(can_bus: can.Bus, log_path: Path) -> None:
+    """Replay a log of CAN messages. this will block the current thread.
+
+    :param can_bus: The CAN bus to use.
+    :param log_path: The path to the log file.
+    """
+    if not log_path.exists():
+        raise FileNotFoundError(f"Log file {log_path} does not exist.")
+
+    with can.ASCReader(log_path) as reader:
+        for message in can.MessageSync(reader):
+            can_bus.send(message)

src/driving/can/can_controller.py

@@ -34,9 +34,7 @@ def __init__(self, can_bus: can.Bus) -> None:
         :param can_bus: The CAN bus to use.
         """
         self.bus = can_bus
-        self.bus.set_filters([
-            {"can_id": CANFeedbackIdentifier.SPEED_SENSOR, "can_mask": 0xFFF, "extended": False}
-        ])
+        self.bus.set_filters([{"can_id": CANFeedbackIdentifier.SPEED_SENSOR, "can_mask": 0xFFF, "extended": False}])
 
         self.__listeners = {}
         self.__thread = threading.Thread(target=self.__listen, daemon=True)