Merge branch 'pixeltoxyz' into 'master'

Get depth from pixel

See merge request !22

src/lhw_interfaces/CMakeLists.txt

@@ -52,6 +52,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg_vision/Entities.msg"
   "msg_vision/DeepFaceEmotion.msg"
   "msg_vision/DeepFaceEmotions.msg"
+  "msg_vision/PixelToXYZ.srv"
   DEPENDENCIES builtin_interfaces geometry_msgs sensor_msgs std_msgs
 )
 

src/lhw_interfaces/msg_vision/PixelToXYZ.srv

+int32 row
+int32 col
+---
+float32[3] xyz
\ No newline at end of file

src/lhw_vision/launch/vision.launch.py

@@ -8,29 +8,12 @@ def generate_launch_description():
             package='lhw_vision',
             executable='yolo_node'
         ),
-        Node(
-            package='lhw_vision',
-            executable='bytetrack_node'
-        ),
-        Node(
-            package='lhw_vision',
-            executable='image_to_world_node'
-        ),
-        Node(
-            package='lhw_vision',
-            executable='densesense'
-        ),
         Node(
             package='lhw_vision',
             executable='face_analysis'
         ),
         Node(
             package='lhw_vision',
-            executable='tracker_node'
-        ),
-        Node(
-            package='lhw_vision',
-            executable='yolo_node'
-        )
-        
+            executable='depth_node'
+        )        
     ])

src/lhw_vision/lhw_vision/depth_node.py

+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image,CompressedImage, CameraInfo
+from lhw_interfaces.msg import Entity, Entities
+from lhw_interfaces.srv import PixelToXYZ
+from builtin_interfaces.msg import Time
+from cv_bridge import CvBridge, CvBridgeError
+from collections import OrderedDict
+#from lhw_vision.utils import save_image
+from lhw_vision import VISION_ROOT
+import numpy as np
+import time
+
+class Depth(Node):
+    """Subscribes to various sources of identified entities (YOLO, etc.),
+    give them an ID which is persistent over time, cleans up messy input, fills in when data is missing,
+    and compiles it all onto a published topic ("tracked_entities"). All of the published entities on
+    this topic contain a list of where they originate (might have multiple origins)
+    and their original IDs so that the original data might be queried by other parts of the system.
+    """
+    def __init__(self):
+        super().__init__("depth_node")
+        self.log = self.get_logger()
+        self.log.info("Starting depth node...")
+        self.depth_sub = self.create_subscription(Image, '/depth/image_rect',#/depth_registered/hw_registered/image_rect',
+                                                     self.depth_callback, 10)
+        self.image_sub = self.create_subscription(CameraInfo, '/camera/depth/camera_info',#TODO fix camera info msg
+                                                     self.camera_intrinsic_callback, 10)
+        self.srv = self.create_service(PixelToXYZ, 'pixel_to_xyz', self.get_xyz_callback)         
+        self.camera_intrinsic = None
+    
+    def depth_callback(self, image_msg: Image):
+        """ Callback upon pepper image being updated. If entities with a corresponding timestamp has been received, self.track is called.
+        If not, then add to self.images
+        Args:
+            image_msg: Image from Pepper
+        """
+        z = np.ndarray(shape=(image_msg.height, image_msg.width),
+                           dtype=np.float32, buffer=image_msg.data)
+        self.z = z
+        self.log.info("got depth")
+        #self.log.info(f"{z=}")
+        #self.log.info(str(self.get_xyz_callback({"c":100,"r":100})))
+
+    def camera_intrinsic_callback(self, camera_msg: None):
+        intrinsic = np.array(camera_msg.k).reshape(3,3)
+        self.inv_intrinsics = np.linalg.inv(intrinsic)
+        self.log.info(f"got intrinsic{intrinsic}")
+
+    def get_xyz_callback(self, pixel):
+        row, col = pixel.r, pixel.c
+        x_im = np.array((col,row,1.))
+        x_n = self.inv_intrinsics @ x_im
+        x_world = x_n
+        x_world[2] = self.z[row, col]
+        #self.log.info(str(self.z.shape))
+        self.log.info('Incoming request\n r: %d c: %d' % (row, col))
+        return x_world
+
+def main(args=None):
+    rclpy.init(args=args)
+    tracker_node = Depth()
+
+    rclpy.spin(tracker_node)
+
+    tracker_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/lhw_vision/lhw_vision/transform_node.py

@@ -22,7 +22,7 @@ from tf2_ros.buffer import Buffer
 from tf2_ros.transform_listener import TransformListener
 
 
-class WaitsForTransform(Node):
+class OpticalFrameToBaseFootprintNone(Node):
     """
     Wait for a transform using callbacks.
     This class is an example of waiting for transforms.
@@ -75,7 +75,7 @@ class WaitsForTransform(Node):
 
 def main():
     rclpy.init()
-    node = WaitsForTransform()
+    node = OpticalFrameToBaseFootprintNone()
     try:
         rclpy.spin(node)
     except KeyboardInterrupt:

src/lhw_vision/setup.py

@@ -36,7 +36,8 @@ setup(
             'webcam_node = lhw_vision.webcam_publisher:main',
             'visualizer = lhw_vision.visualizer:main',
             'debug_image_node = lhw_vision.debug_image_node:main',
-            'debug_tracker_node = lhw_vision.debug_tracker_node:main'
+            'debug_tracker_node = lhw_vision.debug_tracker_node:main',
+            'depth_node = lhw_vision.depth_node:main',
         ],
     },
 )