diff --git a/config/default.yaml b/config/default.yaml index f5f1493..e4d4157 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -1,7 +1,7 @@ depth_anything_3: ros__parameters: # Input RGB image topic - image_topic: "/camera/color/image_raw" + image_topic: "/image_raw" # Output depth image topic depth_image_topic: "/depth" diff --git a/depth_anything_3_ros2/depth_anything_node.py b/depth_anything_3_ros2/depth_anything_node.py index bb65d99..a3dc29b 100644 --- a/depth_anything_3_ros2/depth_anything_node.py +++ b/depth_anything_3_ros2/depth_anything_node.py @@ -1,84 +1,94 @@ #!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image -from cv_bridge import CvBridge +from lib import utils +import cv2 import numpy as np +import rclpy import torch -import cv2 +from cv_bridge import CvBridge from PIL import Image as PILImage +from rclpy.node import Node +from sensor_msgs.msg import Image class DepthAnything3Node(Node): """ROS2 Node for Depth Anything 3 monocular depth estimation.""" def __init__(self): - super().__init__('depth_anything_3_node') + super().__init__("depth_anything_3_node") # Declare parameters - self.declare_parameter('image_topic', '/camera/color/image_raw') - self.declare_parameter('depth_image_topic', '/depth') - self.declare_parameter('device', 'cuda:0') - self.declare_parameter('model_name', 'depth-anything/DA3-Large') + self.declare_parameter("image_topic", "/camera/color/image_raw") + self.declare_parameter("depth_image_topic", "/depth") + self.declare_parameter("device", "cuda:0") + self.declare_parameter("model_name", "depth-anything/DA3-Large") # Get parameters - self.image_topic = self.get_parameter('image_topic').value - self.depth_image_topic = self.get_parameter('depth_image_topic').value - self.device = self.get_parameter('device').value - self.model_name = self.get_parameter('model_name').value + self.image_topic = self.get_parameter("image_topic").value + self.depth_image_topic = self.get_parameter("depth_image_topic").value + self.device = self.get_parameter("device").value + self.model_name = self.get_parameter("model_name").value - self.get_logger().info(f'Initializing Depth Anything 3 Node') - self.get_logger().info(f'Input topic: {self.image_topic}') - self.get_logger().info(f'Output topic: {self.depth_image_topic}') - self.get_logger().info(f'Device: {self.device}') - self.get_logger().info(f'Model: {self.model_name}') + self.get_logger().info("Initializing Depth Anything 3 Node...") + self.get_logger().info(f"Input topic: {self.image_topic}") + self.get_logger().info(f"Output topic: {self.depth_image_topic}") + self.get_logger().info(f"Device: {self.device}") + self.get_logger().info(f"Model: {self.model_name}") # Initialize CV Bridge self.bridge = CvBridge() + # set colormap used for testing + self.colormap = "turbo" + # Load Depth Anything 3 model try: from depth_anything_3.api import DepthAnything3 - self.get_logger().info('Loading Depth Anything 3 model...') + + self.get_logger().info("Loading Depth Anything 3 model...") self.model = DepthAnything3.from_pretrained(self.model_name) # Set device - if 'cuda' in self.device and torch.cuda.is_available(): + if "cuda" in self.device and torch.cuda.is_available(): self.model = self.model.to(self.device) - self.get_logger().info(f'Model loaded on {self.device}') + self.get_logger().info(f"Model loaded on {self.device}") else: - self.model = self.model.to('cpu') - self.device = 'cpu' - self.get_logger().info('CUDA not available, using CPU') + self.model = self.model.to("cpu") + self.device = "cpu" + self.get_logger().info("CUDA not available, using CPU") except ImportError: - self.get_logger().error('Could not import depth_anything_3. Please install it first.') - self.get_logger().error('pip install git+https://github.com/ByteDance-Seed/Depth-Anything-3.git') + self.get_logger().error( + ("Could not import depth_anything_3.", " Please install it first.") + ) + self.get_logger().error( + ( + "pip install git", + "https://github.com/ByteDance-Seed/Depth-Anything-3.git", + ) + ) raise except Exception as e: - self.get_logger().error(f'Failed to load model: {str(e)}') + self.get_logger().error(f"Failed to load model: {str(e)}") raise # Create subscriber and publisher - self.subscription = self.create_subscription( - Image, - self.image_topic, - self.image_callback, - 10) + self.rgb_sub_ = self.create_subscription( + Image, self.image_topic, self.image_callback, 10 + ) - self.publisher = self.create_publisher( - Image, - self.depth_image_topic, - 10) + self.depth_pub_ = self.create_publisher(Image, self.depth_image_topic, 10) + self.colored_pub_ = self.create_publisher( + Image, self.depth_image_topic + "_colored", 10 + ) - self.get_logger().info('Depth Anything 3 Node initialized successfully') + self.get_logger().info("Depth Anything 3 Node initialized.") def image_callback(self, msg): """Callback function for processing incoming images.""" try: # Convert ROS Image message to OpenCV image - cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") # Convert BGR to RGB rgb_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) @@ -91,7 +101,7 @@ def image_callback(self, msg): prediction = self.model.inference([pil_image]) # Extract depth map - depth = prediction['depth'][0] # Get first image depth [H, W] + depth = prediction.depth[0] # Get first image depth [H, W] # Normalize depth for visualization (convert to uint16) # DA3 outputs metric or relative depth depending on model variant @@ -103,19 +113,29 @@ def image_callback(self, msg): depth_max = depth_normalized.max() if depth_max > depth_min: - depth_scaled = ((depth_normalized - depth_min) / (depth_max - depth_min) * 65535.0).astype(np.uint16) + depth_scaled = ( + (depth_normalized - depth_min) / (depth_max - depth_min) * 65535.0 + ).astype(np.uint16) else: depth_scaled = np.zeros_like(depth_normalized, dtype=np.uint16) # Convert to ROS Image message - depth_msg = self.bridge.cv2_to_imgmsg(depth_scaled, encoding='16UC1') + depth_msg = self.bridge.cv2_to_imgmsg(depth_scaled, encoding="16UC1") depth_msg.header = msg.header # Preserve timestamp and frame_id - # Publish depth image - self.publisher.publish(depth_msg) + colored_depth_normalized = utils.colorize_depth( + depth_normalized, colormap=self.colormap + ) + depth_colored_msg = self.bridge.cv2_to_imgmsg( + colored_depth_normalized, encoding="bgr8" + ) + depth_colored_msg.header = msg.header + # Publish depth image + self.depth_pub_.publish(depth_msg) + self.colored_pub_.publish(depth_colored_msg) except Exception as e: - self.get_logger().error(f'Error processing image: {str(e)}') + self.get_logger().error(f"Error processing image: {str(e)}") def main(args=None): @@ -127,11 +147,11 @@ def main(args=None): except KeyboardInterrupt: pass except Exception as e: - print(f'Error: {e}') + print(f"Error: {e}") finally: if rclpy.ok(): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/launch/default.launch.py b/launch/default.launch.py index 9e6e1d0..7241a1e 100644 --- a/launch/default.launch.py +++ b/launch/default.launch.py @@ -10,52 +10,45 @@ def generate_launch_description(): """Generate launch description for Depth Anything 3 ROS2 node.""" # Get package directory - pkg_dir = get_package_share_directory('depth_anything_3_ros2') - config_file = os.path.join(pkg_dir, 'config', 'default.yaml') + pkg_dir = get_package_share_directory("depth_anything_3_ros2") + config_file = os.path.join(pkg_dir, "config", "default.yaml") # Declare launch arguments - image_topic_arg = DeclareLaunchArgument( - 'image_topic', - default_value='/camera/color/image_raw', - description='Input RGB image topic' - ) - - depth_image_topic_arg = DeclareLaunchArgument( - 'depth_image_topic', - default_value='/depth', - description='Output depth image topic' - ) - - device_arg = DeclareLaunchArgument( - 'device', - default_value='cuda:0', - description='Device to run inference on (cuda:0, cpu, etc.)' - ) - - model_name_arg = DeclareLaunchArgument( - 'model_name', - default_value='depth-anything/DA3-Large', - description='Depth Anything 3 model name' - ) + # image_topic_arg = DeclareLaunchArgument( + # "image_topic", + # default_value="/camera/color/image_raw", + # description="Input RGB image topic", + # ) + + # depth_image_topic_arg = DeclareLaunchArgument( + # "depth_image_topic", + # default_value="/depth", + # description="Output depth image topic", + # ) + + # device_arg = DeclareLaunchArgument( + # "device", + # default_value="cuda:0", + # description="Device to run inference on (cuda:0, cpu, etc.)", + # ) + + # model_name_arg = DeclareLaunchArgument( + # "model_name", + # default_value="depth-anything/DA3-Large", + # description="Depth Anything 3 model name", + # ) # Create node depth_anything_node = Node( - package='depth_anything_3_ros2', - executable='depth_anything_node', - name='depth_anything_3', - output='screen', - parameters=[{ - 'image_topic': LaunchConfiguration('image_topic'), - 'depth_image_topic': LaunchConfiguration('depth_image_topic'), - 'device': LaunchConfiguration('device'), - 'model_name': LaunchConfiguration('model_name'), - }] + package="depth_anything_3_ros2", + executable="depth_anything_node", + name="depth_anything_3", + output="screen", + parameters=[config_file], ) - return LaunchDescription([ - image_topic_arg, - depth_image_topic_arg, - device_arg, - model_name_arg, - depth_anything_node - ]) + return LaunchDescription( + [ + depth_anything_node, + ] + ) diff --git a/setup.py b/setup.py index 66350a4..9e312a2 100644 --- a/setup.py +++ b/setup.py @@ -2,29 +2,29 @@ from glob import glob import os -package_name = 'depth_anything_3_ros2' +package_name = "depth_anything_3_ros2" setup( name=package_name, - version='1.0.0', + version="1.0.0", packages=[package_name], data_files=[ - ('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, 'config'), glob('config/*.yaml')), + ("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, "config"), glob("config/*.yaml")), + (os.path.join("lib", package_name, "lib"), glob("lib/*.py")), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='Simon Pesch', - maintainer_email='hello@nomito.de', - description='ROS2 wrapper for Depth Anything 3 monocular depth estimation', - license='Apache-2.0', - tests_require=['pytest'], + maintainer="Simon Pesch", + maintainer_email="hello@nomito.de", + description="ROS2 wrapper for Depth Anything 3 monocular depth estimation", + license="Apache-2.0", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'depth_anything_node = depth_anything_3_ros2.depth_anything_node:main' + "console_scripts": [ + "depth_anything_node = depth_anything_3_ros2.depth_anything_node:main" ], }, )