Merge branch 'main' into hepromark/tilt

.github/workflows/build_base_images.yml

@@ -17,7 +17,9 @@ jobs:
         uses: actions/checkout@v4
 
       - name: Setup Watod Environment
-        run: ./watod_scripts/watod-setup-env.sh
+        run: |
+          MODULES_DIR="$GITHUB_WORKSPACE/modules"
+          . ./watod_scripts/watod-setup-env.sh
         shell: bash
 
       - name: Generate Docker Environment

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "src/perception/lane_detection/src/lane_detection_tensorrt"]
+    path = src/perception/lane_detection/src/lane_detection_tensorrt
+    url = [email protected]:WATonomous/lane_detection_tensorrt.git

README.md

@@ -1,6 +1,6 @@
 # WATonomous Monorepo v2
 
-Dockerized ROS2 setup for the WATonomous Autonomous Driving Software Pipeline
+Dockerized ROS2 setup for the WATonomous Autonomous Vehicle Software Pipeline
 
 - [WATonomous Monorepo](#watonomous-monorepo)
   - [Getting Started](#getting-started)
@@ -88,7 +88,7 @@ wato_monorepo_v2
 ### Using Foxglove
 [Foxglove](https://foxglove.dev/) is used to visualize ROS messages on a local machine.
 
-Add `data_stream` as an `ACTIVE_MODULES` and `watod up`.
+Add `infrastructure` as an `ACTIVE_MODULES` and `watod up`.
 
 It exposes the port specified by the `FOXGLOVE_BRIDGE_PORT` variable, which you will need to forward to your local machine. This can either be done in the `ports` section of VS Code or by running the command `ssh -L 8765:localhost:8765 <username>@<machine>-ubuntu1.watocluster.local` on your local machine.
 

docker/base/inject_wato_base.Dockerfile

@@ -5,12 +5,15 @@ ARG GENERIC_IMAGE
 # for the wato_monorepo.
 FROM ${GENERIC_IMAGE} as wato_base
 
-ENV DEBIAN_FRONTEND noninteractive
+RUN DEBIAN_FRONTEND=noninteractive apt-get update && \
+    apt-get install -y curl sudo && \
+    rm -rf /var/lib/apt/lists/*
+
 ENV USER="bolty"
 ENV AMENT_WS=/home/${USER}/ament_ws
 
 # User Setup
-RUN apt-get update && apt-get install -y curl sudo && \
+RUN DEBIAN_FRONTEND=noninteractive apt-get update && apt-get install -y curl sudo && \
     rm -rf /var/lib/apt/lists/*
 
 # Add a user so that created files in the docker container are owned by a non-root user (for prod)
@@ -19,14 +22,14 @@ RUN addgroup --gid 1000 ${USER} && \
     useradd -rm -d /home/${USER} -s /bin/bash -g ${USER} -G sudo -u 1000 ${USER} -p "$(openssl passwd -6 $USER_PASSWD)"
 
 # install apt-fast
-RUN apt-get -qq update && \
+RUN DEBIAN_FRONTEND=noninteractive apt-get -qq update && \
     apt-get install -qq -y wget && \
     mkdir -p /etc/apt/keyrings && \
     wget -O - "https://keyserver.ubuntu.com/pks/lookup?op=get&search=0xA2166B8DE8BDC3367D1901C11EE2FF37CA8DA16B" \
         | gpg --dearmor -o /etc/apt/keyrings/apt-fast.gpg && \
     echo "deb [signed-by=/etc/apt/keyrings/apt-fast.gpg] \
         http://ppa.launchpad.net/apt-fast/stable/ubuntu focal main" > /etc/apt/sources.list.d/apt-fast.list && \
-    apt-get update -qq && apt-get install -qq -y apt-fast && \
+    DEBIAN_FRONTEND=noninteractive apt-get update -qq && DEBIAN_FRONTEND=noninteractive apt-get install -qq -y apt-fast && \
     echo debconf apt-fast/maxdownloads string 16 | debconf-set-selections && \
     echo debconf apt-fast/dlflag boolean true | debconf-set-selections && \
     echo debconf apt-fast/aptmanager string apt-get | debconf-set-selections

docker/perception/lane_detection/lane_detection.Dockerfile

@@ -1,13 +1,13 @@
-ARG BASE_IMAGE=ghcr.io/watonomous/wato_monorepo/base:humble-ubuntu22.04
-
+ARG BASE_IMAGE=ghcr.io/watonomous/wato_monorepo/base:cuda12.2-humble-ubuntu22.04-devel
+ARG RUNTIME_IMAGE=ghcr.io/watonomous/wato_monorepo/base:cuda12.2-humble-ubuntu22.04
 ################################ Source ################################
 FROM ${BASE_IMAGE} as source
 
 WORKDIR ${AMENT_WS}/src
 
 # Copy in source code 
 COPY src/perception/lane_detection lane_detection
-COPY src/wato_msgs/sample_msgs sample_msgs
+COPY src/wato_msgs/perception_msgs/lane_detection_msgs lane_detection_msgs
 
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
@@ -19,6 +19,13 @@ RUN apt-get -qq update && rosdep update && \
 ################################# Dependencies ################################
 FROM ${BASE_IMAGE} as dependencies
 
+RUN apt-get update && apt-get install -y libopencv-dev \
+        python3-opencv \
+        tensorrt \
+        cuda-toolkit
+
+RUN export OpenCV_DIR=/usr/lib/x86_64-linux-gnu/cmake/opencv4/OpenCVConfig.cmake
+
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
 RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
@@ -46,7 +53,21 @@ COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh
 ENTRYPOINT ["./wato_ros_entrypoint.sh"]
 
 ################################ Prod ################################
-FROM build as deploy
+# Use a different runtime image for a smaller image size
+FROM ${RUNTIME_IMAGE} as deploy
+
+# Install runtime libs
+RUN apt-get update && apt-get install -y \
+    ros-humble-cv-bridge \
+    tensorrt
+
+# Copy the compiled binary to the runtime image
+COPY --from=build ${AMENT_WS} ${AMENT_WS}
+
+WORKDIR ${AMENT_WS}
+
+COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh
+ENTRYPOINT ["./wato_ros_entrypoint.sh"]
 
 # Source Cleanup and Security Setup
 RUN chown -R $USER:$USER ${AMENT_WS}

docker/perception/radar_object_detection/radar_object_detection.Dockerfile

@@ -7,7 +7,7 @@ WORKDIR ${AMENT_WS}/src
 
 # Copy in source code 
 COPY src/perception/radar_object_detection radar_object_detection
-COPY src/wato_msgs/radar_msgs radar_msgs
+COPY src/wato_msgs/perception_msgs/radar_msgs radar_msgs
 
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \

docs/dev/rosbag.md

@@ -5,9 +5,9 @@ More on bags can be found here: https://docs.ros.org/en/humble/Tutorials/Beginne
 
 ## To Use
 
-Add `data_stream` as an `ACTIVE_PROFILE` in `watod-config.sh`. 
+Add `infrastructure` as an `ACTIVE_PROFILE` in `watod-config.sh`. 
 
-Run `watod up` (or however you want to launch the `data_stream` service). 
+Run `watod up` (or however you want to launch the `infrastructure` service). 
 
 The working directory of the `data_stream` container should have a `nuscenes` directory, which contains the NuScenes dataset converted to ros2bag format. To confirm this, run `watod run data_stream ls nuscenes` to view the available bags. Each bag has its own directory. The location of the `.mcap` file is `<name>/<name>_0.mcap`. For example, one of the bags is in `nuscenes/NuScenes-v1.0-mini-scene-0061/NuScenes-v1.0-mini-scene-0061_0.mcap`. 
 

docs/setup/setup.md

@@ -27,7 +27,7 @@ Note: These are all available on WATonomous servers by default.
 ## Running Your First Program in the Monorepo
 These steps will let you run our sample nodes module. To run other modules, refer to our modules documentation.
 
-1. Clone this repo onto the host machine by using `$ git clone [email protected]:WATonomous/wato_monorepo.git`. We recommend you clone the repo into your home directory, `~`
+1. Clone this repo onto the host machine by using `$ git clone --recurse-submodules [email protected]:WATonomous/wato_monorepo.git`. The `--recurse-submodules` flag is needed to initialize submodules required by the repo - if you did not clone with this flag, run `git submodule update --init --recursive` inside the repo. We recommend you clone the repo into your home directory, `~`
 2. Login to the our container registry by using `docker login ghcr.io`. Provide your GitHub Username and GitHub Token.
 3. Configure to only run our sample nodes by uncommenting and setting `ACTIVE_MODULES="samples"` in `watod-config.sh`
 4. Run `$ ./watod pull` to pull latest docker images from our container registry.

modules/docker-compose.perception.yaml

@@ -29,7 +29,7 @@ services:
             - driver: nvidia
               count: 1
               capabilities: [ gpu ]
-    command: /bin/bash -c "ros2 launch camera_object_detection eve_launch.py"
+    command: /bin/bash -c "ros2 launch camera_object_detection eve.launch.py"
     volumes:
       - /mnt/wato-drive2/perception_models/yolov8m.pt:/perception_models/yolov8m.pt
       - /mnt/wato-drive2/perception_models/traffic_light.pt:/perception_models/traffic_light.pt
@@ -66,7 +66,16 @@ services:
         - "${PERCEPTION_LANE_DETECTION_IMAGE}:build_main"
       target: deploy
     image: "${PERCEPTION_LANE_DETECTION_IMAGE}:${TAG}"
-    command: /bin/bash -c "ros2 launch lane_detection lane_detection.launch.py"
+    command: /bin/bash -c "ros2 launch lane_detection eve.launch.py"
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: 1
+              capabilities: [gpu]
+    volumes:
+      - /mnt/wato-drive2/perception-weights/tensorrt_model/ultra_fast_lane_detection_v2/resource/model/ufldv2_culane_res34_320x1600.onnx:/models/ufldv2_culane_res34_320x1600.onnx
 
   tracking:
     build:

src/perception/lane_detection/CMakeLists.txt

@@ -1,14 +1,70 @@
 cmake_minimum_required(VERSION 3.8)
-project(lane_detection)
+set(ProjectName "lane_detection")
+set(CUDA_TOOLKIT_ROOT_DIR "/usr/local/cuda")
+SET(CMAKE_CUDA_COMPILER /usr/local/cuda-12.2/bin/nvcc)
+SET(CUDACXX /usr/local/cuda-12.2/bin/nvcc)
+project(${ProjectName} LANGUAGES CXX CUDA)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+### TensorRT model ###
+
+# Select build system and set compile options
+include(${CMAKE_CURRENT_LIST_DIR}/src/lane_detection_tensorrt/common_helper/cmakes/build_setting.cmake)
+add_executable(lane_detection src/lane_detection_node.cpp)
+
+add_subdirectory(./src/lane_detection_tensorrt/ultra_fast_lane_detection_v2/image_processor image_processor)
+target_include_directories(${ProjectName} PUBLIC 
+    ${CMAKE_CURRENT_SOURCE_DIR}/include 
+    ./src/lane_detection_tensorrt/ultra_fast_lane_detection_v2/image_processor)
+target_link_libraries(${ProjectName} ImageProcessor)
+
+find_package(OpenCV REQUIRED)
+target_include_directories(${ProjectName} PUBLIC ${OpenCV_INCLUDE_DIRS})
+target_link_libraries(${ProjectName} ${OpenCV_LIBS})
+
+# Copy resouce
+add_definitions(-DRESOURCE_DIR="/models/")
+
+### End of TensorRT model ###
+
+set(REQUIRED_PACKAGES
+  rclcpp
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  lane_detection_msgs
+  cv_bridge
+  image_transport
+  CUDA
+)
+
+foreach(PKG IN LISTS REQUIRED_PACKAGES)
+  find_package(${PKG} REQUIRED)
+endforeach()
+
+# Install launch files
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+# Install config files
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(TARGETS
+lane_detection
+  DESTINATION lib/${PROJECT_NAME})
 
+ament_target_dependencies(lane_detection rclcpp OpenCV std_msgs sensor_msgs cv_bridge image_transport lane_detection_msgs)
 ament_package()

src/perception/lane_detection/config/eve_config.yaml

@@ -0,0 +1,9 @@
+lane_detection_node:
+  ros__parameters:
+    camera_topic: /camera/left/image_color
+    publish_vis_topic: /camera/left/lanes_viz
+    publish_lanes_topic: /camera/left/lanes
+    save_images: false
+    save_images_path: /tmp
+    publish_source_image: false
+    debug_node: false

src/perception/lane_detection/config/nuscenes_config.yaml

@@ -0,0 +1,9 @@
+lane_detection_node:
+  ros__parameters:
+    camera_topic: /CAM_FRONT/image_rect_compressed
+    publish_vis_topic: /CAM_FRONT/lanes_viz
+    publish_lanes_topic: /CAM_FRONT/lanes
+    save_images: false
+    save_images_path: /tmp
+    publish_source_image: false
+    debug_node: false

src/perception/lane_detection/include/lane_detection_node.hpp

@@ -0,0 +1,48 @@
+#ifndef LANE_DETECTION_NODE_HPP
+#define LANE_DETECTION_NODE_HPP
+
+#include <chrono>
+#include <filesystem>
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <cv_bridge/cv_bridge.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/compressed_image.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include "lane_detection_msgs/msg/lane_detection.hpp"
+
+#include <opencv2/opencv.hpp>
+
+#include "common_helper_cv.h"
+#include "image_processor.h"
+
+class LaneDetectionNode : public rclcpp::Node {
+ public:
+  LaneDetectionNode();
+  void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr &msg);
+
+ private:
+  void populate_lane_msg(lane_detection_msgs::msg::LaneDetection &lane_msg,
+                         const std::vector<std::vector<float>> &raw_lane_list);
+  void save_image(const cv::Mat &image, const std::string &filename);
+
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
+  rclcpp::Publisher<lane_detection_msgs::msg::LaneDetection>::SharedPtr lane_detection_pub_;
+
+  size_t count_;
+
+  // ROS Parameters
+  std::string camera_topic_;
+  std::string publish_vis_topic_;
+  std::string publish_lanes_topic_;
+  bool save_images_;
+  std::string save_dir_;
+  bool publish_source_image_;
+  // Debug will publish the source image with lane detection overlay
+  bool debug_node_;
+};
+
+#endif  // LANE_DETECTION_NODE_HPP

src/perception/lane_detection/launch/eve.launch.py

@@ -0,0 +1,26 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    config = os.path.join(
+        get_package_share_directory('lane_detection'),
+        'config',
+        'eve_config.yaml'
+    )
+
+    lane_detection_node = Node(
+        package='lane_detection',
+        executable='lane_detection',
+        name='lane_detection_node',
+        parameters=[config],
+        arguments=['--ros-args', '--log-level', 'info']
+    )
+
+    ld.add_action(lane_detection_node)
+
+    return ld

src/perception/lane_detection/launch/nuscenes.launch.py

@@ -0,0 +1,26 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    config = os.path.join(
+        get_package_share_directory('lane_detection'),
+        'config',
+        'nuscenes_config.yaml'
+    )
+
+    lane_detection_node = Node(
+        package='lane_detection',
+        executable='lane_detection',
+        name='lane_detection_node',
+        parameters=[config],
+        arguments=['--ros-args', '--log-level', 'info']
+    )
+
+    ld.add_action(lane_detection_node)
+
+    return ld