Merge pull request #96 from WATonomous/j89leung/depth-estimation-star…

…ter-node

Add depth estimation ROS 2 node starter code

docker/perception/depth_estimation/depth_estimation.Dockerfile

@@ -0,0 +1,54 @@
+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/depth_estimation depth_estimation
+
+# 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/<ROS_DISTRO>/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}

modules/dev_overrides/docker-compose.perception.yaml

@@ -64,3 +64,13 @@ services:
     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

modules/docker-compose.perception.yaml

@@ -78,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"

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()

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

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<sensor_msgs::msg::Image>::SharedPtr image_subscription_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth_image_publisher_;
+};
+
+#endif  // DEPTH_ESTIMATION_NODE_HPP_

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

src/perception/depth_estimation/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>depth_estimation</name>
+  <version>0.0.0</version>
+  <description>Depth estimation ROS 2 node</description>
+  <maintainer email="[email protected]">TBD</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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<std::string>("camera_topic", "/camera/image_raw");
+  this->declare_parameter<std::string>("depth_map_topic", "/depth/image_raw");
+  this->declare_parameter<bool>("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<sensor_msgs::msg::Image>(
+      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<sensor_msgs::msg::Image>(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<DepthEstimationNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

watod_scripts/watod-setup-env.sh

@@ -72,6 +72,7 @@ PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE=${PERCEPTION_CAMERA_OBJECT_DETECTION_IM
 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"}
@@ -166,6 +167,7 @@ echo "PERCEPTION_CAMERA_OBJECT_DETECTION_IMAGE=$PERCEPTION_CAMERA_OBJECT_DETECTI
 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"