diff --git a/docker/perception/depth_estimation/depth_estimation.Dockerfile b/docker/perception/depth_estimation/depth_estimation.Dockerfile new file mode 100644 index 00000000..d51497ae --- /dev/null +++ b/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//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.perception.yaml b/modules/dev_overrides/docker-compose.perception.yaml index 3a1a522d..4dcf2693 100644 --- a/modules/dev_overrides/docker-compose.perception.yaml +++ b/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 diff --git a/modules/docker-compose.perception.yaml b/modules/docker-compose.perception.yaml index 75d9d165..d30474f9 100644 --- a/modules/docker-compose.perception.yaml +++ b/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" 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/depth_estimation/package.xml b/src/perception/depth_estimation/package.xml new file mode 100644 index 00000000..76fc56c8 --- /dev/null +++ b/src/perception/depth_estimation/package.xml @@ -0,0 +1,16 @@ + + + + depth_estimation + 0.0.0 + Depth estimation ROS 2 node + TBD + TODO: License declaration + + ament_cmake + + + + 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/watod_scripts/watod-setup-env.sh b/watod_scripts/watod-setup-env.sh index 029e9233..a165c028 100755 --- a/watod_scripts/watod-setup-env.sh +++ b/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"