import rclpy
from rclpy.node import Node

from interfaces.msg import PoseArrayClassAndID


class SubPose(Node):
    def __init__(self, name):
        super().__init__(name)
        self.sub = self.create_subscription(PoseArrayClassAndID, '/pose/cv_detect_pose', self.pose_callback, 10)

    def pose_callback(self, msg):
        pose_list = []
        for object in msg.objects:
            pose_list.append(
                {
                    "class_id": object.class_id,
                    "class_name": object.class_name,
                    "pose": object.pose
                }
            )

        self.get_logger().info(f'{pose_list}')


def main(args=None):
    rclpy.init(args=args)
    node = SubPose('pose')
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
