diff --git a/.github/templates/docker_context/docker_context.sh b/.github/templates/docker_context/docker_context.sh index 126b5cb2..e8dc0d1f 100755 --- a/.github/templates/docker_context/docker_context.sh +++ b/.github/templates/docker_context/docker_context.sh @@ -36,6 +36,12 @@ while read -r module; do # Loop through each service while read -r service_out; do + # Temporarily skip perception services that have too large image size + if [[ "$service_out" == "lane_detection" ]] || \ + [[ "$service_out" == "camera_object_detection" ]] || \ + [[ "$service_out" == "semantic_segmentation" ]]; then + continue + fi # Construct JSON object for each service with module and service name json_object=$(jq -nc --arg module_out "$module_out" --arg service_out "$service_out" \ '{module: $module_out, service: $service_out}') diff --git a/.github/workflows/linting_auto.yml b/.github/workflows/linting_auto.yml index a02b3b0c..a2b7646f 100644 --- a/.github/workflows/linting_auto.yml +++ b/.github/workflows/linting_auto.yml @@ -8,6 +8,7 @@ on: branches: - main types: + - unlabeled - labeled - synchronize @@ -51,4 +52,4 @@ jobs: repo: context.repo.repo, issue_number: context.issue.number, name: 'auto-lint' - }); \ No newline at end of file + }); diff --git a/docker/perception/traffic_sign_detection/traffic_sign_detection.Dockerfile b/docker/perception/depth_estimation/depth_estimation.Dockerfile similarity index 94% rename from docker/perception/traffic_sign_detection/traffic_sign_detection.Dockerfile rename to docker/perception/depth_estimation/depth_estimation.Dockerfile index a07d0507..d51497ae 100644 --- a/docker/perception/traffic_sign_detection/traffic_sign_detection.Dockerfile +++ b/docker/perception/depth_estimation/depth_estimation.Dockerfile @@ -6,8 +6,7 @@ FROM ${BASE_IMAGE} as source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/perception/traffic_sign_detection traffic_sign_detection -COPY src/wato_msgs/sample_msgs sample_msgs +COPY src/perception/depth_estimation depth_estimation # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/perception/traffic_light_detection/traffic_light_detection.Dockerfile b/docker/perception/traffic_light_detection/traffic_light_detection.Dockerfile deleted file mode 100644 index d91b79e5..00000000 --- a/docker/perception/traffic_light_detection/traffic_light_detection.Dockerfile +++ /dev/null @@ -1,55 +0,0 @@ -ARG BASE_IMAGE=ghcr.io/watonomous/wato_monorepo/base:humble-ubuntu22.04 - -################################ Source ################################ -FROM ${BASE_IMAGE} as source - -WORKDIR ${AMENT_WS}/src - -# Copy in source code -COPY src/perception/traffic_light_detection traffic_light_detection -COPY src/wato_msgs/sample_msgs sample_msgs - -# Scan for rosdeps -RUN apt-get -qq update && rosdep update && \ - rosdep install --from-paths . --ignore-src -r -s \ - | grep 'apt-get install' \ - | awk '{print $3}' \ - | sort > /tmp/colcon_install_list - -################################# Dependencies ################################ -FROM ${BASE_IMAGE} as dependencies - -# 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) - -# Copy in source code from source stage -WORKDIR ${AMENT_WS} -COPY --from=source ${AMENT_WS}/src src - -# Dependency Cleanup -WORKDIR / -RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \ - rm -rf /root/* /root/.ros /tmp/* /var/lib/apt/lists/* /usr/share/doc/* - -################################ Build ################################ -FROM dependencies as build - -# Build ROS2 packages -WORKDIR ${AMENT_WS} -RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ - colcon build \ - --cmake-args -DCMAKE_BUILD_TYPE=Release - -# Entrypoint will run before any CMD on launch. Sources ~/opt//setup.bash and ~/ament_ws/install/setup.bash -COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh -ENTRYPOINT ["./wato_ros_entrypoint.sh"] - -################################ Prod ################################ -FROM build as deploy - -# Source Cleanup and Security Setup -RUN chown -R $USER:$USER ${AMENT_WS} -RUN rm -rf src/* - -USER ${USER} diff --git a/modules/dev_overrides/docker-compose.action.yaml b/modules/dev_overrides/docker-compose.action.yaml index 1318b6bc..1984e6db 100644 --- a/modules/dev_overrides/docker-compose.action.yaml +++ b/modules/dev_overrides/docker-compose.action.yaml @@ -10,6 +10,7 @@ services: extends: file: ../docker-compose.action.yaml service: global_planning + image: "${ACTION_GLOBAL_PLANNING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/action/global_planning:/home/ament_ws/src/global_planning @@ -19,6 +20,7 @@ services: extends: file: ../docker-compose.action.yaml service: behaviour_planning + image: "${ACTION_BEHAVIOUR_PLANNING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/action/behaviour_planning:/home/ament_ws/src/behaviour_planning @@ -28,6 +30,7 @@ services: extends: file: ../docker-compose.action.yaml service: local_planning + image: "${ACTION_LOCAL_PLANNING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/action/local_planning:/home/ament_ws/src/local_planning @@ -37,6 +40,7 @@ services: extends: file: ../docker-compose.action.yaml service: model_predictive_control + image: "${ACTION_MPC_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/action/model_predictive_control:/home/ament_ws/src/model_predictive_control \ No newline at end of file diff --git a/modules/dev_overrides/docker-compose.interfacing.yaml b/modules/dev_overrides/docker-compose.interfacing.yaml index 503469d9..12a5db5b 100644 --- a/modules/dev_overrides/docker-compose.interfacing.yaml +++ b/modules/dev_overrides/docker-compose.interfacing.yaml @@ -10,6 +10,7 @@ services: extends: file: ../docker-compose.interfacing.yaml service: sensor_interfacing + image: "${INTERFACING_SENSOR_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/sensor_interfacing:/home/bolty/ament_ws/src/sensor_interfacing @@ -19,6 +20,7 @@ services: extends: file: ../docker-compose.interfacing.yaml service: can_interfacing + image: "${INTERFACING_CAN_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/can_interfacing:/home/bolty/ament_ws/src/can_interfacing diff --git a/modules/dev_overrides/docker-compose.perception.yaml b/modules/dev_overrides/docker-compose.perception.yaml index 31b41896..4dcf2693 100644 --- a/modules/dev_overrides/docker-compose.perception.yaml +++ b/modules/dev_overrides/docker-compose.perception.yaml @@ -10,6 +10,7 @@ services: extends: file: ../docker-compose.perception.yaml service: radar_object_detection + image: "${PERCEPTION_RADAR_OBJECT_DETECTION_IMAGE:?}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/perception/radar_object_detection:/home/bolty/ament_ws/src/radar_object_detection @@ -19,6 +20,7 @@ services: extends: file: ../docker-compose.perception.yaml service: camera_object_detection + image: "${PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE:?}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/perception/camera_object_detection:/home/bolty/ament_ws/src/camera_object_detection @@ -28,33 +30,17 @@ services: extends: file: ../docker-compose.perception.yaml service: lidar_object_detection + image: "${PERCEPTION_LIDAR_OBJECT_DETECTION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/perception/lidar_object_detection:/home/bolty/ament_ws/src/lidar_object_detection - traffic_light_detection: - <<: *fixuid - extends: - file: ../docker-compose.perception.yaml - service: traffic_light_detection - command: tail -F anything - volumes: - - ${MONO_DIR}/src/perception/traffic_light_detection:/home/bolty/ament_ws/src/traffic_light_detection - - traffic_sign_detection: - <<: *fixuid - extends: - file: ../docker-compose.perception.yaml - service: traffic_sign_detection - command: tail -F anything - volumes: - - ${MONO_DIR}/src/perception/traffic_sign_detection:/home/bolty/ament_ws/src/traffic_sign_detection - semantic_segmentation: <<: *fixuid extends: file: ../docker-compose.perception.yaml service: semantic_segmentation + image: "${PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/perception/semantic_segmentation:/home/bolty/ament_ws/src/semantic_segmentation @@ -64,6 +50,7 @@ services: extends: file: ../docker-compose.perception.yaml service: lane_detection + image: "${PERCEPTION_LANE_DETECTION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/perception/lane_detection:/home/bolty/ament_ws/src/lane_detection @@ -73,6 +60,17 @@ services: extends: file: ../docker-compose.perception.yaml service: tracking + image: "${PERCEPTION_TRACKING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/perception/tracking:/home/bolty/ament_ws/src/tracking + + depth_estimation: + <<: *fixuid + extends: + file: ../docker-compose.perception.yaml + service: tracking + image: "${PERCEPTION_DEPTH_ESTIMATION_IMAGE}:build_${TAG}" + command: tail -F anything + volumes: + - ${MONO_DIR}/src/perception/depth_estimation:/home/bolty/ament_ws/src/depth_estimation diff --git a/modules/dev_overrides/docker-compose.samples.yaml b/modules/dev_overrides/docker-compose.samples.yaml index 2d49b7bf..335da171 100644 --- a/modules/dev_overrides/docker-compose.samples.yaml +++ b/modules/dev_overrides/docker-compose.samples.yaml @@ -10,6 +10,7 @@ services: extends: file: ../docker-compose.samples.yaml service: cpp_aggregator + image: "${SAMPLES_CPP_AGGREGATOR_IMAGE:?}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/samples/cpp/aggregator:/home/bolty/ament_ws/src/aggregator @@ -19,6 +20,8 @@ services: # extends: # file: ../docker-compose.samples.yaml # service: py_aggregator + # image: "${SAMPLES_PYTHON_AGGREGATOR_IMAGE:?}:build_${TAG}" + # command: tail -F anything # volumes: # - ${MONO_DIR}/src/samples/python/aggregator:/home/bolty/ament_ws/src/aggregator @@ -27,6 +30,8 @@ services: # extends: # file: ../docker-compose.samples.yaml # service: cpp_producer + # image: "${SAMPLES_CPP_PRODUCER_IMAGE:?}:build_${TAG}" + # command: tail -F anything # volumes: # - ${MONO_DIR}/src/samples/cpp/producer:/home/bolty/ament_ws/src/producer @@ -35,6 +40,7 @@ services: extends: file: ../docker-compose.samples.yaml service: py_producer + image: "${SAMPLES_PYTHON_PRODUCER_IMAGE:?}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/samples/python/producer:/home/bolty/ament_ws/src/producer @@ -44,6 +50,8 @@ services: # extends: # file: ../docker-compose.samples.yaml # service: cpp_transformer + # image: "${SAMPLES_CPP_TRANSFORMER_IMAGE:?}:build_${TAG}" + # command: tail -F anything # volumes: # - ${MONO_DIR}/src/samples/cpp/transformer:/home/bolty/ament_ws/src/transformer @@ -52,6 +60,7 @@ services: extends: file: ../docker-compose.samples.yaml service: py_transformer + image: "${SAMPLES_PYTHON_TRANSFORMER_IMAGE:?}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/samples/python/transformer:/home/bolty/ament_ws/src/transformer diff --git a/modules/dev_overrides/docker-compose.world_modeling.yaml b/modules/dev_overrides/docker-compose.world_modeling.yaml index 67b03241..7682c63a 100644 --- a/modules/dev_overrides/docker-compose.world_modeling.yaml +++ b/modules/dev_overrides/docker-compose.world_modeling.yaml @@ -10,6 +10,7 @@ services: extends: file: ../docker-compose.world_modeling.yaml service: hd_map + image: "${WORLD_MODELING_HD_MAP_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/hd_map @@ -19,6 +20,7 @@ services: extends: file: ../docker-compose.world_modeling.yaml service: localization + image: "${WORLD_MODELING_LOCALIZATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/localization @@ -28,6 +30,7 @@ services: extends: file: ../docker-compose.world_modeling.yaml service: occupancy + image: "${WORLD_MODELING_OCCUPANCY_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/occupancy @@ -37,6 +40,7 @@ services: extends: file: ../docker-compose.world_modeling.yaml service: occupancy_segmentation + image: "${WORLD_MODELING_OCCUPANCY_SEGMENTATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/occupancy_segmentation @@ -46,6 +50,7 @@ services: extends: file: ../docker-compose.world_modeling.yaml service: motion_forecasting + image: "${WORLD_MODELING_MOTION_FORECASTING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/motion_forecasting diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml index 2923c8f3..d30474f9 100644 --- a/modules/docker-compose.perception.yaml +++ b/modules/docker-compose.perception.yaml @@ -21,6 +21,7 @@ services: - "${PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE:?}:build_main" target: deploy image: "${PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE:?}:${TAG}" + shm_size: 8G deploy: resources: reservations: @@ -31,6 +32,8 @@ services: 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 + - /mnt/wato-drive2/perception_models/traffic_signs_v0.pt:/perception_models/traffic_signs_v1.pt lidar_object_detection: build: @@ -42,27 +45,6 @@ services: target: deploy image: "${PERCEPTION_LIDAR_OBJECT_DETECTION_IMAGE}:${TAG}" command: /bin/bash -c "ros2 launch lidar_object_detection lidar_object_detection.launch.py" - traffic_light_detection: - build: - context: .. - dockerfile: docker/perception/traffic_light_detection/traffic_light_detection.Dockerfile - cache_from: - - "${PERCEPTION_TRAFFIC_LIGHT_DETECTION_IMAGE}:build_${TAG}" - - "${PERCEPTION_TRAFFIC_LIGHT_DETECTION_IMAGE}:build_main" - target: deploy - image: "${PERCEPTION_TRAFFIC_LIGHT_DETECTION_IMAGE}:${TAG}" - command: /bin/bash -c "ros2 launch traffic_light_detection traffic_light_detection.launch.py" - - traffic_sign_detection: - build: - context: .. - dockerfile: docker/perception/traffic_sign_detection/traffic_sign_detection.Dockerfile - cache_from: - - "${PERCEPTION_TRAFFIC_SIGN_DETECTION_IMAGE}:build_${TAG}" - - "${PERCEPTION_TRAFFIC_SIGN_DETECTION_IMAGE}:build_main" - target: deploy - image: "${PERCEPTION_TRAFFIC_SIGN_DETECTION_IMAGE}:${TAG}" - command: /bin/bash -c "ros2 launch traffic_sign_detection traffic_sign_detection.launch.py" semantic_segmentation: build: @@ -96,3 +78,14 @@ services: target: deploy image: "${PERCEPTION_TRACKING_IMAGE}:${TAG}" command: /bin/bash -c "ros2 launch tracking tracking.launch.py" + + depth_estimation: + build: + context: .. + dockerfile: docker/perception/depth_estimation/depth_estimation.Dockerfile + cache_from: + - "${PERCEPTION_DEPTH_ESTIMATION_IMAGE}:build_${TAG}" + - "${PERCEPTION_DEPTH_ESTIMATION_IMAGE}:build_main" + target: deploy + image: "${PERCEPTION_DEPTH_ESTIMATION_IMAGE}:${TAG}" + command: /bin/bash -c "ros2 launch depth_estimation eve.launch.py" diff --git a/src/perception/camera_object_detection/camera_object_detection/yolov8_detection.py b/src/perception/camera_object_detection/camera_object_detection/yolov8_detection.py index f5822173..05ceb431 100755 --- a/src/perception/camera_object_detection/camera_object_detection/yolov8_detection.py +++ b/src/perception/camera_object_detection/camera_object_detection/yolov8_detection.py @@ -1,5 +1,6 @@ import rclpy from rclpy.node import Node +import os from sensor_msgs.msg import Image, CompressedImage from vision_msgs.msg import ( @@ -9,7 +10,7 @@ ) from ultralytics.nn.autobackend import AutoBackend -from ultralytics.data.augment import LetterBox +from ultralytics.data.augment import LetterBox, CenterCrop from ultralytics.utils.ops import non_max_suppression from ultralytics.utils.plotting import Annotator, colors @@ -37,18 +38,24 @@ def __init__(self): self.declare_parameter("model_path", "/perception_models/yolov8m.pt") self.declare_parameter("image_size", 1024) self.declare_parameter("compressed", False) + self.declare_parameter("crop_mode", "LetterBox") + self.declare_parameter("save_detections", False) self.camera_topic = self.get_parameter("camera_topic").value self.publish_vis_topic = self.get_parameter("publish_vis_topic").value - self.publish_detection_topic = self.get_parameter( - "publish_detection_topic").value + self.publish_detection_topic = self.get_parameter("publish_detection_topic").value self.model_path = self.get_parameter("model_path").value self.image_size = self.get_parameter("image_size").value self.compressed = self.get_parameter("compressed").value + self.crop_mode = self.get_parameter("crop_mode").value + self.save_detections = bool(self.get_parameter("save_detections").value) + self.counter = 0 # For saving detections + if self.save_detections: + if not os.path.exists("detections"): + os.makedirs("detections") self.line_thickness = 1 self.half = False - self.augment = False self.subscription = self.create_subscription( Image if not self.compressed else CompressedImage, @@ -61,9 +68,11 @@ def __init__(self): ), ) + self.orig_image_width = None + self.orig_image_height = None + # set device - self.device = torch.device( - "cuda" if torch.cuda.is_available() else "cpu") + self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") if torch.cuda.is_available(): self.get_logger().info("Using GPU for inference") else: @@ -73,24 +82,69 @@ def __init__(self): self.cv_bridge = CvBridge() # load yolov8 model - self.model = AutoBackend( - self.model_path, device=self.device, dnn=False, fp16=False) + self.model = AutoBackend(self.model_path, device=self.device, dnn=False, fp16=False) - self.names = self.model.module.names if hasattr( - self.model, "module") else self.model.names + self.names = self.model.module.names if hasattr(self.model, "module") else self.model.names self.stride = int(self.model.stride) # setup vis publishers - self.vis_publisher = self.create_publisher( - Image, self.publish_vis_topic, 10) + self.vis_publisher = self.create_publisher(Image, self.publish_vis_topic, 10) self.detection_publisher = self.create_publisher( - Detection2DArray, self.publish_detection_topic, 10) + Detection2DArray, self.publish_detection_topic, 10 + ) self.get_logger().info( - f"Successfully created node listening on camera topic: {self.camera_topic}...") + f"Successfully created node listening on camera topic: {self.camera_topic}..." + ) + + def crop_image(self, cv_image): + if self.crop_mode == "LetterBox": + img = LetterBox(self.image_size, stride=self.stride)(image=cv_image) + elif self.crop_mode == "CenterCrop": + img = CenterCrop(self.image_size)(cv_image) + else: + raise Exception("Invalid crop mode, please choose either 'LetterBox' or 'CenterCrop'!") + + return img + + def convert_bboxes_to_orig_frame(self, bbox): + """ + Converts bounding box coordinates from the scaled image frame back to the original image frame. + + This function takes into account the original image dimensions and the scaling method used + (either "LetterBox" or "CenterCrop") to accurately map the bounding box coordinates back to + their original positions in the original image. + + Parameters: + bbox (list): A list containing the bounding box coordinates in the format [x1, y1, w1, h1] + in the scaled image frame. + + Returns: + list: A list containing the bounding box coordinates in the format [x1, y1, w1, h1] + in the original image frame. - def preprocess_image(self, cv_image): + """ + width_scale = self.orig_image_width / self.image_size + height_scale = self.orig_image_height / self.image_size + if self.crop_mode == "LetterBox": + translation = (self.image_size - self.orig_image_height / width_scale) / 2 + return [ + bbox[0] * width_scale, + (bbox[1] - translation) * width_scale, + bbox[2] * width_scale, + bbox[3] * width_scale, + ] + elif self.crop_mode == "CenterCrop": + translation = (self.orig_image_width / height_scale - self.image_size) / 2 + return [ + (bbox[0] + translation) * height_scale, + bbox[1] * height_scale, + bbox[2] * height_scale, + bbox[3] * height_scale, + ] + + def crop_and_convert_to_tensor(self, cv_image): """ Preprocess the image by resizing, padding and rearranging the dimensions. @@ -100,9 +154,7 @@ def preprocess_image(self, cv_image): Returns: torch.Tensor image for model input of shape (1,3,w,h) """ - # Padded resize - img = cv_image - img = LetterBox(self.image_size, stride=self.stride)(image=cv_image) + img = self.crop_image(cv_image) # Convert img = img.transpose(2, 0, 1) @@ -143,10 +195,11 @@ def postprocess_detections(self, detections, annotator): annotator_img = annotator.result() return (processed_detections, annotator_img) - def publish_vis(self, annotated_img, feed): + def publish_vis(self, annotated_img, msg, feed): # Publish visualizations imgmsg = self.cv_bridge.cv2_to_imgmsg(annotated_img, "bgr8") - imgmsg.header.frame_id = "camera_{}_link".format(feed) + imgmsg.header.stamp = msg.header.stamp + imgmsg.header.frame_id = msg.header.frame_id self.vis_publisher.publish(imgmsg) def publish_detections(self, detections, msg, feed): @@ -175,10 +228,13 @@ def publish_detections(self, detections, msg, feed): detection2darray.detections.append(detection2d) self.detection_publisher.publish(detection2darray) - return def image_callback(self, msg): self.get_logger().debug("Received image") + if self.orig_image_width is None: + self.orig_image_width = msg.width + self.orig_image_height = msg.height + images = [msg] # msg is a single sensor image startTime = time.time() for image in images: @@ -189,16 +245,13 @@ def image_callback(self, msg): cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) else: try: - cv_image = self.cv_bridge.imgmsg_to_cv2( - image, desired_encoding="passthrough") + cv_image = self.cv_bridge.imgmsg_to_cv2(image, desired_encoding="passthrough") except CvBridgeError as e: self.get_logger().error(str(e)) return # preprocess image and run through prediction - img = self.preprocess_image(cv_image) - processed_cv_image = LetterBox( - self.image_size, stride=self.stride)(image=cv_image) + img = self.crop_and_convert_to_tensor(cv_image) pred = self.model(img) # nms function used same as yolov8 detect.py @@ -217,6 +270,7 @@ def image_callback(self, msg): xyxy[3] - xyxy[1], ] bbox = [b.item() for b in bbox] + bbox = self.convert_bboxes_to_orig_frame(bbox) detections.append( { @@ -228,20 +282,24 @@ def image_callback(self, msg): self.get_logger().debug(f"{label}: {bbox}") annotator = Annotator( - processed_cv_image, + cv_image, line_width=self.line_thickness, example=str(self.names), ) - (detections, annotated_img) = self.postprocess_detections( - detections, annotator) + (detections, annotated_img) = self.postprocess_detections(detections, annotator) # Currently we support a single camera so we pass an empty string feed = "" - self.publish_vis(annotated_img, feed) + self.publish_vis(annotated_img, msg, feed) self.publish_detections(detections, msg, feed) + if self.save_detections: + cv2.imwrite(f"detections/{self.counter}.jpg", annotated_img) + self.counter += 1 + self.get_logger().info( - f"Finished in: {time.time() - startTime}, {1/(time.time() - startTime)} Hz") + f"Finished in: {time.time() - startTime}, {1/(time.time() - startTime)} Hz" + ) def main(args=None): diff --git a/src/perception/camera_object_detection/config/eve_config.yaml b/src/perception/camera_object_detection/config/eve_config.yaml index efac49fa..208ccbe6 100755 --- a/src/perception/camera_object_detection/config/eve_config.yaml +++ b/src/perception/camera_object_detection/config/eve_config.yaml @@ -1,23 +1,23 @@ left_camera_object_detection_node: ros__parameters: camera_topic: /camera/left/image_color - publish_vis_topic: /camera/left/annotated_img - publish_detection_topic: /camera/left/detections + publish_vis_topic: /camera/left/camera_detections_viz + publish_detection_topic: /camera/left/camera_detections model_path: /perception_models/yolov8m.pt image_size: 1024 center_camera_object_detection_node: ros__parameters: camera_topic: /camera/center/image_color - publish_vis_topic: /camera/center/annotated_img - publish_detection_topic: /camera/center/detections + publish_vis_topic: /camera/center/camera_detections_viz + publish_detection_topic: /camera/center/camera_detections model_path: /perception_models/yolov8m.pt image_size: 1024 right_camera_object_detection_node: ros__parameters: camera_topic: /camera/right/image_color - publish_vis_topic: /camera/right/annotated_img - publish_detection_topic: /camera/right/detections + publish_vis_topic: /camera/right/camera_detections_viz + publish_detection_topic: /camera/right/camera_detections model_path: /perception_models/yolov8m.pt image_size: 1024 diff --git a/src/perception/camera_object_detection/config/traffic_light_config.yaml b/src/perception/camera_object_detection/config/traffic_light_config.yaml new file mode 100755 index 00000000..9a3e00c8 --- /dev/null +++ b/src/perception/camera_object_detection/config/traffic_light_config.yaml @@ -0,0 +1,9 @@ +traffic_light_node: + ros__parameters: + camera_topic: /camera/left/image_color + publish_vis_topic: /traffic_lights_viz + publish_detection_topic: /traffic_lights + model_path: /perception_models/traffic_light.pt + crop_mode: CenterCrop + image_size: 1024 + save_detections: false diff --git a/src/perception/camera_object_detection/config/traffic_signs_config.yaml b/src/perception/camera_object_detection/config/traffic_signs_config.yaml new file mode 100644 index 00000000..8a143a77 --- /dev/null +++ b/src/perception/camera_object_detection/config/traffic_signs_config.yaml @@ -0,0 +1,9 @@ +traffic_signs_node: + ros__parameters: + camera_topic: /camera/left/image_color + publish_vis_topic: /traffic_signs_viz + publish_detection_topic: /traffic_signs + model_path: /perception_models/traffic_signs_v1.pt + crop_mode: CenterCrop + image_size: 1024 + save_detections: false diff --git a/src/perception/camera_object_detection/launch/deepracer_launch.py b/src/perception/camera_object_detection/launch/deepracer.launch.py similarity index 82% rename from src/perception/camera_object_detection/launch/deepracer_launch.py rename to src/perception/camera_object_detection/launch/deepracer.launch.py index c853a556..138c32b4 100755 --- a/src/perception/camera_object_detection/launch/deepracer_launch.py +++ b/src/perception/camera_object_detection/launch/deepracer.launch.py @@ -5,14 +5,12 @@ def generate_launch_description(): - ld = LaunchDescription() config = os.path.join( get_package_share_directory('camera_object_detection'), 'config', 'deeepracer.yaml' ) - # nodes camera_object_detection_node = Node( package='camera_object_detection', executable='camera_object_detection_node', @@ -20,7 +18,4 @@ def generate_launch_description(): parameters=[config] ) - # finalize - ld.add_action(camera_object_detection_node) - - return ld + return LaunchDescription([camera_object_detection_node]) diff --git a/src/perception/camera_object_detection/launch/eve.launch.py b/src/perception/camera_object_detection/launch/eve.launch.py new file mode 100755 index 00000000..9ff4992e --- /dev/null +++ b/src/perception/camera_object_detection/launch/eve.launch.py @@ -0,0 +1,57 @@ +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.conditions import LaunchConfigurationEquals +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + launch_traffic_light = LaunchConfiguration("launch_traffic_light", default=True) + launch_traffic_light_arg = DeclareLaunchArgument( + "launch_traffic_light", + default_value=launch_traffic_light, + description="Launch traffic light detection", + ) + launch_traffic_signs = LaunchConfiguration("launch_traffic_signs", default=True) + launch_traffic_signs_arg = DeclareLaunchArgument( + "launch_traffic_signs", + default_value=launch_traffic_signs, + description="Launch traffic signs detection", + ) + + launch_args = [launch_traffic_light_arg, launch_traffic_signs_arg] + + camera_object_detection_launch_include_dir = os.path.join( + get_package_share_directory("camera_object_detection"), "launch", "include" + ) + + pretrained_yolov8_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [camera_object_detection_launch_include_dir, "/pretrained_yolov8.launch.py"] + ), + ) + + traffic_light_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [camera_object_detection_launch_include_dir, "/traffic_light.launch.py"] + ), + condition=LaunchConfigurationEquals("launch_traffic_light", "True"), + ) + + traffic_signs_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [camera_object_detection_launch_include_dir, "/traffic_signs.launch.py"] + ), + condition=LaunchConfigurationEquals("launch_traffic_signs", "True"), + ) + + return LaunchDescription( + launch_args + + [ + pretrained_yolov8_launch, + traffic_light_launch, + traffic_signs_launch, + ] + ) diff --git a/src/perception/camera_object_detection/launch/eve_launch.py b/src/perception/camera_object_detection/launch/eve_launch.py deleted file mode 100755 index 94ac35df..00000000 --- a/src/perception/camera_object_detection/launch/eve_launch.py +++ /dev/null @@ -1,42 +0,0 @@ -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('camera_object_detection'), - 'config', - 'eve_config.yaml' - ) - - # nodes - left_camera_object_detection_node = Node( - package='camera_object_detection', - executable='camera_object_detection_node', - name='left_camera_object_detection_node', - parameters=[config] - ) - - center_camera_object_detection_node = Node( - package='camera_object_detection', - executable='camera_object_detection_node', - name='center_camera_object_detection_node', - parameters=[config] - ) - - right_camera_object_detection_node = Node( - package='camera_object_detection', - executable='camera_object_detection_node', - name='right_camera_object_detection_node', - parameters=[config] - ) - - # finalize - ld.add_action(left_camera_object_detection_node) - ld.add_action(center_camera_object_detection_node) - ld.add_action(right_camera_object_detection_node) - - return ld diff --git a/src/perception/camera_object_detection/launch/include/pretrained_yolov8.launch.py b/src/perception/camera_object_detection/launch/include/pretrained_yolov8.launch.py new file mode 100644 index 00000000..13096462 --- /dev/null +++ b/src/perception/camera_object_detection/launch/include/pretrained_yolov8.launch.py @@ -0,0 +1,39 @@ +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(): + config = os.path.join( + get_package_share_directory("camera_object_detection"), "config", "eve_config.yaml" + ) + + left_camera_object_detection_node = Node( + package="camera_object_detection", + executable="camera_object_detection_node", + name="left_camera_object_detection_node", + parameters=[config], + ) + + center_camera_object_detection_node = Node( + package="camera_object_detection", + executable="camera_object_detection_node", + name="center_camera_object_detection_node", + parameters=[config], + ) + + right_camera_object_detection_node = Node( + package="camera_object_detection", + executable="camera_object_detection_node", + name="right_camera_object_detection_node", + parameters=[config], + ) + + return LaunchDescription( + [ + left_camera_object_detection_node, + center_camera_object_detection_node, + right_camera_object_detection_node, + ] + ) diff --git a/src/perception/camera_object_detection/launch/include/traffic_light.launch.py b/src/perception/camera_object_detection/launch/include/traffic_light.launch.py new file mode 100755 index 00000000..234844b8 --- /dev/null +++ b/src/perception/camera_object_detection/launch/include/traffic_light.launch.py @@ -0,0 +1,21 @@ +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(): + config = os.path.join( + get_package_share_directory('camera_object_detection'), + 'config', + 'traffic_light_config.yaml' + ) + + camera_object_detection_node = Node( + package='camera_object_detection', + executable='camera_object_detection_node', + name='traffic_light_node', + parameters=[config] + ) + + return LaunchDescription([camera_object_detection_node]) diff --git a/src/perception/camera_object_detection/launch/include/traffic_signs.launch.py b/src/perception/camera_object_detection/launch/include/traffic_signs.launch.py new file mode 100644 index 00000000..cdb0134f --- /dev/null +++ b/src/perception/camera_object_detection/launch/include/traffic_signs.launch.py @@ -0,0 +1,21 @@ +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(): + config = os.path.join( + get_package_share_directory('camera_object_detection'), + 'config', + 'traffic_signs_config.yaml' + ) + + camera_object_detection_node = Node( + package='camera_object_detection', + executable='camera_object_detection_node', + name='traffic_signs_node', + parameters=[config] + ) + + return LaunchDescription([camera_object_detection_node]) diff --git a/src/perception/camera_object_detection/launch/nuscenes_launch.py b/src/perception/camera_object_detection/launch/nuscenes.launch.py similarity index 87% rename from src/perception/camera_object_detection/launch/nuscenes_launch.py rename to src/perception/camera_object_detection/launch/nuscenes.launch.py index 7a366260..faff24b6 100755 --- a/src/perception/camera_object_detection/launch/nuscenes_launch.py +++ b/src/perception/camera_object_detection/launch/nuscenes.launch.py @@ -12,7 +12,6 @@ def generate_launch_description(): 'nuscenes_config.yaml' ) - # nodes camera_object_detection_node = Node( package='camera_object_detection', executable='camera_object_detection_node', @@ -21,7 +20,4 @@ def generate_launch_description(): arguments=['--ros-args', '--log-level', 'info'] ) - # finalize - ld.add_action(camera_object_detection_node) - - return ld + return LaunchDescription([camera_object_detection_node]) diff --git a/src/perception/camera_object_detection/setup.py b/src/perception/camera_object_detection/setup.py index 7ba35c15..c9665f79 100755 --- a/src/perception/camera_object_detection/setup.py +++ b/src/perception/camera_object_detection/setup.py @@ -12,7 +12,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'launch', 'include'), glob('launch/include/*.launch.py')), (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), ], install_requires=['setuptools'], diff --git a/src/perception/depth_estimation/CMakeLists.txt b/src/perception/depth_estimation/CMakeLists.txt new file mode 100644 index 00000000..fb6f76f5 --- /dev/null +++ b/src/perception/depth_estimation/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.5) +project(depth_estimation) + +# 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_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +include_directories(include) + +add_executable(depth_estimation_node src/depth_estimation_node.cpp) +ament_target_dependencies(depth_estimation_node rclcpp sensor_msgs) + +install(TARGETS + depth_estimation_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/perception/depth_estimation/config/config.yaml b/src/perception/depth_estimation/config/config.yaml new file mode 100644 index 00000000..4f42e0b3 --- /dev/null +++ b/src/perception/depth_estimation/config/config.yaml @@ -0,0 +1,5 @@ +depth_estimation_node: + ros__parameters: + camera_topic: "/camera/left/image_color" + depth_map_topic: "/depth_map" + debug_node: false diff --git a/src/perception/depth_estimation/include/depth_estimation_node.hpp b/src/perception/depth_estimation/include/depth_estimation_node.hpp new file mode 100644 index 00000000..e54e9b6f --- /dev/null +++ b/src/perception/depth_estimation/include/depth_estimation_node.hpp @@ -0,0 +1,22 @@ +#ifndef DEPTH_ESTIMATION_NODE_HPP_ +#define DEPTH_ESTIMATION_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" + +class DepthEstimationNode : public rclcpp::Node { + public: + DepthEstimationNode(); + + private: + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); + + std::string camera_topic_; + std::string depth_map_topic_; + bool debug_node_; + + rclcpp::Subscription::SharedPtr image_subscription_; + rclcpp::Publisher::SharedPtr depth_image_publisher_; +}; + +#endif // DEPTH_ESTIMATION_NODE_HPP_ diff --git a/src/perception/depth_estimation/launch/eve.launch.py b/src/perception/depth_estimation/launch/eve.launch.py new file mode 100644 index 00000000..8567b7e6 --- /dev/null +++ b/src/perception/depth_estimation/launch/eve.launch.py @@ -0,0 +1,25 @@ +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('depth_estimation'), + 'config', + 'config.yaml' + ) + + depth_estimation_node = Node( + package='depth_estimation', + executable='depth_estimation_node', + name='depth_estimation_node', + parameters=[config], + arguments=['--ros-args', '--log-level', 'info'] + ) + + ld.add_action(depth_estimation_node) + + return ld diff --git a/src/perception/traffic_sign_detection/package.xml b/src/perception/depth_estimation/package.xml similarity index 70% rename from src/perception/traffic_sign_detection/package.xml rename to src/perception/depth_estimation/package.xml index 48d6de51..76fc56c8 100644 --- a/src/perception/traffic_sign_detection/package.xml +++ b/src/perception/depth_estimation/package.xml @@ -1,10 +1,10 @@ - traffic_sign_detection + depth_estimation 0.0.0 - TODO: Package description - bolty + Depth estimation ROS 2 node + TBD TODO: License declaration ament_cmake diff --git a/src/perception/depth_estimation/src/depth_estimation_node.cpp b/src/perception/depth_estimation/src/depth_estimation_node.cpp new file mode 100644 index 00000000..bebd1ad0 --- /dev/null +++ b/src/perception/depth_estimation/src/depth_estimation_node.cpp @@ -0,0 +1,39 @@ +#include "depth_estimation_node.hpp" +#include "sensor_msgs/msg/image.hpp" + +DepthEstimationNode::DepthEstimationNode() : Node("depth_estimation_node") { + this->declare_parameter("camera_topic", "/camera/image_raw"); + this->declare_parameter("depth_map_topic", "/depth/image_raw"); + this->declare_parameter("deubg_node", false); + + this->get_parameter("camera_topic", camera_topic_); + this->get_parameter("depth_map_topic", depth_map_topic_); + this->get_parameter("debug_node", debug_node_); + + RCLCPP_INFO(this->get_logger(), "debug_node: %s", debug_node_ ? "true" : "false"); + + RCLCPP_INFO(this->get_logger(), "Subscribing to camera topic '%s'", camera_topic_.c_str()); + image_subscription_ = this->create_subscription( + camera_topic_, 10, + std::bind(&DepthEstimationNode::image_callback, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "Publishing depth map to topic '%s'", depth_map_topic_.c_str()); + depth_image_publisher_ = this->create_publisher(depth_map_topic_, 10); +} + +void DepthEstimationNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received sensor image on '%s'", camera_topic_.c_str()); + + // TODO: Process the incoming image and estimate depth + // Replace this empty image with the actual depth map + auto depth_map_image = sensor_msgs::msg::Image(); + depth_image_publisher_->publish(depth_map_image); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/perception/traffic_sign_detection/CMakeLists.txt b/src/perception/traffic_sign_detection/CMakeLists.txt deleted file mode 100644 index b968c4cf..00000000 --- a/src/perception/traffic_sign_detection/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(traffic_sign_detection) - -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( REQUIRED) - -ament_package() diff --git a/watod_scripts/watod-setup-env.sh b/watod_scripts/watod-setup-env.sh index 3da136fe..a165c028 100755 --- a/watod_scripts/watod-setup-env.sh +++ b/watod_scripts/watod-setup-env.sh @@ -69,11 +69,10 @@ INFRASTRUCTURE_FOXGLOVE_IMAGE=${DATA_STREAM_IMAGE:-"$REGISTRY_URL/infrastructure PERCEPTION_RADAR_OBJECT_DETECTION_IMAGE=${PERCEPTION_RADAR_OBJECT_DETECTION_IMAGE:-"$REGISTRY_URL/perception/radar_object_detection"} PERCEPTION_LIDAR_OBJECT_DETECTION_IMAGE=${PERCEPTION_LIDAR_OBJECT_DETECTION_IMAGE:-"$REGISTRY_URL/perception/lidar_object_detection"} PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE=${PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE:-"$REGISTRY_URL/perception/camera_object_detection"} -PERCEPTION_TRAFFIC_LIGHT_DETECTION_IMAGE=${PERCEPTION_TRAFFIC_LIGHT_DETECTION_IMAGE:-"$REGISTRY_URL/perception/traffic_light_detection"} -PERCEPTION_TRAFFIC_SIGN_DETECTION_IMAGE=${PERCEPTION_TRAFFIC_SIGN_DETECTION_IMAGE:-"$REGISTRY_URL/perception/traffic_sign_detection"} PERCEPTION_LANE_DETECTION_IMAGE=${PERCEPTION_LANE_DETECTION_IMAGE:-"$REGISTRY_URL/perception/lane_detection"} PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE=${PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE:-"$REGISTRY_URL/perception/semantic_segmentation"} PERCEPTION_TRACKING_IMAGE=${PERCEPTION_TRACKING_IMAGE:-"$REGISTRY_URL/perception/tracking"} +PERCEPTION_DEPTH_ESTIMATION_IMAGE=${PERCEPTION_DEPTH_ESTIMATION_IMAGE:-"$REGISTRY_URL/perception/depth_estimation"} # World Modeling WORLD_MODELING_HD_MAP_IMAGE=${WORLD_MODELING_HD_MAP_IMAGE:-"$REGISTRY_URL/world_modeling/hd_map"} @@ -165,11 +164,10 @@ echo "INFRASTRUCTURE_FOXGLOVE_IMAGE=$INFRASTRUCTURE_FOXGLOVE_IMAGE" >> "$MODULES echo "PERCEPTION_RADAR_OBJECT_DETECTION_IMAGE=$PERCEPTION_RADAR_OBJECT_DETECTION_IMAGE" >> "$MODULES_DIR/.env" echo "PERCEPTION_LIDAR_OBJECT_DETECTION_IMAGE=$PERCEPTION_LIDAR_OBJECT_DETECTION_IMAGE" >> "$MODULES_DIR/.env" echo "PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE=$PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE" >> "$MODULES_DIR/.env" -echo "PERCEPTION_TRAFFIC_LIGHT_DETECTION_IMAGE=$PERCEPTION_TRAFFIC_LIGHT_DETECTION_IMAGE" >> "$MODULES_DIR/.env" -echo "PERCEPTION_TRAFFIC_SIGN_DETECTION_IMAGE=$PERCEPTION_TRAFFIC_SIGN_DETECTION_IMAGE" >> "$MODULES_DIR/.env" echo "PERCEPTION_LANE_DETECTION_IMAGE=$PERCEPTION_LANE_DETECTION_IMAGE" >> "$MODULES_DIR/.env" echo "PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE=$PERCEPTION_SEMANTIC_SEGMENTATION_IMAGE" >> "$MODULES_DIR/.env" echo "PERCEPTION_TRACKING_IMAGE=$PERCEPTION_TRACKING_IMAGE" >> "$MODULES_DIR/.env" +echo "PERCEPTION_DEPTH_ESTIMATION_IMAGE=$PERCEPTION_DEPTH_ESTIMATION_IMAGE" >> "$MODULES_DIR/.env" # World Modeling echo "WORLD_MODELING_HD_MAP_IMAGE=$WORLD_MODELING_HD_MAP_IMAGE" >> "$MODULES_DIR/.env"