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_controller.common import PyRegionOfIntrest

from rq_hardware.hardware_client import HardwareClient


rclpy.init()

client = HardwareClient(namespace="rq")
roi = PyRegionOfIntrest.dummy()
roi.dimensions_mm = roi.dimensions_mm * 100.
roi.resolution_mm = roi.resolution_mm * 10.
print(roi.dimensions_mm)

future = client.check_reachability_map(roi, np.zeros(3), 10.)

rclpy.spin_until_future_complete(client, future)

volume = client.reachability_map_response_2_py(future.result())

print(volume.shape)

print(volume.array.sum())