import rclpy
from rclpy.node import Node
from shape_msgs.msg import Mesh, MeshTriangle
from geometry_msgs.msg import Point
import numpy as np
import open3d as o3d

from rq_hardware.artist.artist_hardware_service import ArtistHardwareInterface

class MeshPublisher(Node):
    def __init__(self):
        super().__init__('mesh_publisher')
        self.publisher = self.create_publisher(Mesh, 'rq_artist_collision_mesh_mm', 10)

    def publish_mesh(self, msg):
        self.publisher.publish(msg)


rclpy.init()
mesh_publisher = MeshPublisher()
bunny = o3d.data.ArmadilloMesh()
bunny_mesh = o3d.io.read_triangle_mesh(bunny.path)
mesh_publisher.publish_mesh(
    ArtistHardwareInterface.from_open3d_to_message(bunny_mesh))
print(bunny_mesh.get_oriented_bounding_box())



mesh_publisher.destroy_node()
rclpy.shutdown()