Merge branch 'main' into 389-Add-autoware-mini-to-the-research-section

.github/workflows/build.yml

@@ -123,5 +123,5 @@ jobs:
           rm -rf /tmp/.buildx-cache/cache/latest
           mv /tmp/.buildx-cache/cache-new/latest /tmp/.buildx-cache/cache/latest
       
-      - name: Prune all images older than 1 days from self-hosted runner
+      - name: Prune all images older than 1 day from self-hosted runner
         run: docker image prune -a -f --filter "until=24h"

.vscode/extensions.json

@@ -12,6 +12,7 @@
         "richardkotze.git-mob",
         "ms-vscode-remote.remote-containers",
         "valentjn.vscode-ltex",
-        "ms-python.black-formatter"
+        "ms-python.black-formatter",
+        "augustocdias.tasks-shell-input"
     ]
 }

.vscode/launch.json

@@ -0,0 +1,23 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "53000 leaderboard attach",
+            "type": "debugpy",
+            "request": "attach",
+            "connect": {
+                "host": "localhost",
+                "port": 53000
+            },
+            "pathMappings": [
+                {
+                    "localRoot": "${workspaceFolder}/code",
+                    "remoteRoot": "${env:PAF_CATKIN_CODE_ROOT}"
+                }
+            ]
+        }
+    ],
+}

.vscode/tasks.json

@@ -0,0 +1,22 @@
+{
+    "version": "2.0.0",
+    "tasks": [
+        {
+            "label": "Run 'catkin_make' in docker container. Choose container from list of running containers.",
+            "type": "shell",
+            "command": "docker exec -it ${input:container_name} bash -c 'cd ../../catkin_ws && source /opt/ros/noetic/setup.bash && bash devel/setup.bash && catkin_make'",
+            "problemMatcher": [],
+            "detail": "Executes 'catkin_make' selected container. Requires Tasks Shell Input Extension, in order to generate the list of containers.",
+        }
+    ],
+    "inputs": [
+        {
+            "id": "container_name",
+            "type": "command",
+            "command": "shellCommand.execute",
+            "args": {
+                "command": "docker ps --format '{{.Names}}'",
+            }
+        }
+    ]
+}

build/agent_service.yaml

@@ -30,6 +30,7 @@ services:
       - ROS_HOSTNAME=agent
       - XDG_RUNTIME_DIR=/tmp/runtime-carla
       - ROUTE=/opt/leaderboard/data/routes_devtest.xml
+      - DEBUG_WRAPPER_DEFAULT_HOST=0.0.0.0
       # Simple route without special scenarios
       # - ROUTE=/workspace/code/routes/routes_simple.xml
     volumes:

build/docker-compose.leaderboard.yaml

@@ -8,4 +8,11 @@ services:
     extends:
       file: agent_service.yaml
       service: agent
-    command: bash -c "sleep 10 && sudo chown -R ${USER_UID}:${USER_GID} ../ && sudo chmod -R a+w ../ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP"
+    ports:
+      # Reserved ports for the debugger
+      - "53000-53100:53000-53100"
+    command: |-
+      bash -c "sleep 10 && sudo chown -R ${USER_UID}:${USER_GID} ../ && \
+      sudo chmod -R a+w ../ && sudo mkdir -p $${XDG_RUNTIME_DIR} && sudo chmod 0700 $${XDG_RUNTIME_DIR} && sudo chown -R ${USER_UID}:${USER_GID} $${XDG_RUNTIME_DIR} && \
+      (rqt_console &) && disown -a && \
+      python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP"

build/docker/agent/Dockerfile

@@ -162,8 +162,10 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt
 # Add agent code
 COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/
 
+# For debugger
+ENV PAF_CATKIN_CODE_ROOT=/catkin_ws/src/code
 # Link code into catkin workspace
-RUN ln -s /workspace/code /catkin_ws/src
+RUN ln -s -T /workspace/code ${PAF_CATKIN_CODE_ROOT}
 
 # re-make the catkin workspace
 RUN source /opt/ros/noetic/setup.bash && catkin_make

code/acting/src/debug_wrapper.py

@@ -0,0 +1 @@
+../../debug_wrapper.py

code/agent/launch/agent.launch

@@ -21,5 +21,9 @@
         <arg name="role_name" value="$(arg role_name)"/>
     </include>
 
+    <!-- debugging -->
+    <include file="$(find debugging)/launch/debugging.launch">
+    </include>
+
     <node type="rviz" name="rviz" pkg="rviz" args="-d $(find agent)/config/rviz_config.rviz" />
 </launch>

code/debug_wrapper.py

@@ -0,0 +1,201 @@
+#!/usr/bin/env python3
+"""Debug wrapper node
+
+Node that wraps a python ros node
+and is able to open a debugpy remote debugger instance.
+
+Logs any exceptions from the node and other information
+into the ros console via the debug_logger node.
+
+Always tries to at least start the node,
+even if dependencies like debugpy are missing.
+
+Usage:
+    Already done for existing ros packages: symlink this file into the package. Example:
+    `cd code/perception/src && ln -s ../../debug_wrapper.py debug_wrapper.py`
+
+    Adjust the launch configuration to use the debug_wrapper.py
+    instead of the node file and set the required args
+
+    More info in [debugging.md](doc/development/debugging.md)
+
+Arguments:
+    --debug_node: Required: The filename of the node to debug
+    --debug_port: The port the debugger listens on. If not set,
+    --debug_host: The host the debugger binds to.
+        Defaults to the environment variable `DEBUG_WRAPPER_DEFAULT_HOST` if set,
+        otherwise `localhost`
+    --debug_wait: If True, the wrapper waits until a client (VS Code) is connected
+        and only then starts node execution
+
+Raises:
+    ArgumentParserError: Missing required parameters
+    error: If the started debug_node raises an exception,
+        it is logged and then raised again
+"""
+
+
+import importlib.util
+import os
+import runpy
+import sys
+import argparse
+import time
+from multiprocessing.connection import Client
+
+import rospy
+
+NODE_NAME = "NAMEERR"
+
+
+def eprint(msg: str):
+    """Log msg into stderr.
+
+    Used instead of print, because only stderr seems to reliably land in agent.log
+
+    Args:
+        msg (str): Log message
+    """
+    print(f"[debug_wrapper]: {msg}", file=sys.stderr)
+
+
+def log(msg: str, level: str):
+    """Log msg via the debug_logger node
+
+    Args:
+        msg (str): Log message
+        level (str): Log level. One of (debug), info, warn, error, fatal.
+            debug level not recommended, because these are not published to /rosout
+    """
+    error = None
+    success = False
+    start_time = time.monotonic()
+    while not success and start_time + 5.0 > time.monotonic():
+        try:
+            address = ("localhost", 52999)
+            conn = Client(address, authkey=b"debug_logger")
+            conn.send({"name": NODE_NAME, "msg": msg, "level": level})
+            conn.close()
+            success = True
+        except Exception as e:
+            error = e
+    if not success:
+        eprint(msg)
+        if error is not None:
+            eprint(f"Failed to send to logger: {error}")
+
+
+def logfatal(msg: str):
+    log(f"FAILED TO START NODE - NODE WILL NOT SHOW UP: {msg}", "fatal")
+
+
+def logerr(msg: str):
+    log(msg, "error")
+
+
+def logwarn(msg: str):
+    log(msg, "warn")
+
+
+def loginfo(msg: str):
+    log(msg, "info")
+
+
+def run_module_at(path: str):
+    """Runs a python module based on its file path
+
+    Args:
+        path (str): python file path to run
+    """
+    basename = os.path.basename(path)
+    module_dir = os.path.dirname(path)
+    module_name = os.path.splitext(basename)[0]
+    sys.path.append(module_dir)
+    runpy.run_module(module_name, run_name="__main__")
+
+
+def start_debugger(
+    node_module_name: str, host: str, port: int, wait_for_client: bool = False
+):
+    """_summary_
+
+    Args:
+        node_module_name (str): Name of the underlying node. Only used for logging
+        host (str): host the debugger binds to
+        port (int): debugger port
+        wait_for_client (bool, optional): If the debugger should wait
+            for a client to attach. Defaults to False.
+    """
+    debugger_spec = importlib.util.find_spec("debugpy")
+    if debugger_spec is not None:
+        try:
+            import debugpy
+
+            debugpy.listen((host, port))
+            logwarn(f"Started debugger on {host}:{port} for {node_module_name}")
+            if wait_for_client:
+                logwarn("Waiting until debugging client is attached...")
+                debugpy.wait_for_client()
+        except Exception as error:
+            # Yes, all exceptions should be catched and sent into rosconsole
+            logerr(f"Failed to start debugger: {error}")
+    else:
+        logerr("debugpy module not found. Unable to start debugger")
+
+
+class ArgumentParserError(Exception):
+    pass
+
+
+class ThrowingArgumentParser(argparse.ArgumentParser):
+    def error(self, message):
+        logfatal(f"Wrong node arguments. Check launch config. : {message}")
+        raise ArgumentParserError(message)
+
+
+def main(argv):
+    default_host = "localhost"
+    if "DEBUG_WRAPPER_DEFAULT_HOST" in os.environ:
+        default_host = os.environ["DEBUG_WRAPPER_DEFAULT_HOST"]
+
+    node_args = rospy.myargv(argv=argv)
+    parser = ThrowingArgumentParser(
+        prog="debug wrapper",
+    )
+    parser.add_argument("--debug_node", required=True, type=str)
+    parser.add_argument("--debug_port", required=False, type=int)
+    parser.add_argument("--debug_host", default=default_host, type=str)
+    parser.add_argument("--debug_wait", action="store_true")
+    args, unknown_args = parser.parse_known_args(node_args)
+
+    debug_node = args.debug_node
+    global NODE_NAME
+    NODE_NAME = debug_node
+    base_dir = os.path.abspath(os.path.dirname(__file__))
+
+    if args.debug_port is not None:
+        start_debugger(
+            args.debug_node,
+            args.debug_host,
+            args.debug_port,
+            wait_for_client=args.debug_wait,
+        )
+    else:
+        logerr(
+            """Missing parameter to start debugger: --debug_port
+                Add it in the launch configuration"""
+        )
+
+    target_type_path = os.path.join(base_dir, debug_node)
+    loginfo(f"Node {args.debug_node} starting at {base_dir}")
+    try:
+        run_module_at(target_type_path)
+    except BaseException as error:
+        # Yes, all exceptions including SystemExit should be catched.
+        # We want to always know when a node exits
+        logfatal(f"Failed to run node {debug_node}: {error}")
+        raise error
+
+
+if __name__ == "__main__":
+    main(argv=sys.argv)