diff --git a/src/lhw_interfaces/CMakeLists.txt b/src/lhw_interfaces/CMakeLists.txt index 87a117d3e4f9591b26cd56ddbb14f8e3140bbe82..e2adce7c34b6025b80f3b2f23060a3cca722e4ca 100644 --- a/src/lhw_interfaces/CMakeLists.txt +++ b/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 ) diff --git a/src/lhw_interfaces/msg_vision/PixelToXYZ.srv b/src/lhw_interfaces/msg_vision/PixelToXYZ.srv new file mode 100644 index 0000000000000000000000000000000000000000..63a2cf5fd29a144ce86867927889f9dbff024de5 --- /dev/null +++ b/src/lhw_interfaces/msg_vision/PixelToXYZ.srv @@ -0,0 +1,4 @@ +int32 row +int32 col +--- +float32[3] xyz \ No newline at end of file diff --git a/src/lhw_vision/launch/vision.launch.py b/src/lhw_vision/launch/vision.launch.py index 49d65163b823dc7e974f70cc825569f121d74212..f7e9bbb39148c47b5be0535e2959363bf6503034 100644 --- a/src/lhw_vision/launch/vision.launch.py +++ b/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' + ) ]) diff --git a/src/lhw_vision/lhw_vision/depth_node.py b/src/lhw_vision/lhw_vision/depth_node.py new file mode 100644 index 0000000000000000000000000000000000000000..7200f32e85ee9961d48ff3e87b78de3cd41626ea --- /dev/null +++ b/src/lhw_vision/lhw_vision/depth_node.py @@ -0,0 +1,71 @@ +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() diff --git a/src/lhw_vision/lhw_vision/transform_node.py b/src/lhw_vision/lhw_vision/transform_node.py index e9add9bb8ed2d871c7a7575c23ae2a5ab3ef79d7..126988163551b4c61f1d3dfb2ca821df7eb68f06 100644 --- a/src/lhw_vision/lhw_vision/transform_node.py +++ b/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: diff --git a/src/lhw_vision/setup.py b/src/lhw_vision/setup.py index 7d6d4ad06cc993f2a5a080322f0dec0e4135202d..ab82c1e51dfe1a36b7016e844cc7b912ad34e260 100644 --- a/src/lhw_vision/setup.py +++ b/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', ], }, )