Skip to content

Instantly share code, notes, and snippets.

@RichardKelley
Created January 29, 2024 18:19
Show Gist options
  • Save RichardKelley/e85b22dde77914f78d834a055738a73b to your computer and use it in GitHub Desktop.
Save RichardKelley/e85b22dde77914f78d834a055738a73b to your computer and use it in GitHub Desktop.
A ROS2 node that uses Depth Anything to get depth from a monocular camera in real time.
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
import torch
from transformers import AutoImageProcessor, AutoModelForDepthEstimation
class DepthAnythingNode(Node):
def __init__(self):
super().__init__('depth_anything')
self.subscription = self.create_subscription(
Image,
'/flir_camera/image_raw',
self.image_callback,
10)
self.subscription
self.bridge = CvBridge()
self.depth_publisher = self.create_publisher(Image, "depth_anything/depth", 10)
self.processor = AutoImageProcessor.from_pretrained("LiheYoung/depth-anything-small-hf")
self.model = AutoModelForDepthEstimation.from_pretrained("LiheYoung/depth-anything-small-hf").to("cuda")
def image_callback(self, msg):
frame = self.bridge.imgmsg_to_cv2(msg)
frame = cv2.resize(frame, (512, 384))
#cv2.imshow("rgb", frame)
#cv2.waitKey(1)
inputs = self.processor(images=frame, return_tensors="pt").to("cuda")
with torch.no_grad():
outputs = self.model(**inputs)
depth = outputs.predicted_depth
depth = depth.squeeze().cpu().numpy()
output = (depth * 255 / np.max(depth)).astype("uint8")
self.depth_publisher.publish(self.bridge.cv2_to_imgmsg(output))
#cv2.imshow("depth", output)
#cv2.waitKey(1)
def main(args = None):
rclpy.init(args=args)
image_node = DepthAnythingNode()
rclpy.spin(image_node)
image_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment