add node script to use happypose with real time camera image

scripts/happypose_with_camera.py

+from happypose.pose_estimators.cosypose.cosypose.scripts.run_inference_on_example import (
+    run_inference,
+)
+from happypose.pose_estimators.cosypose.cosypose.utils.cosypose_wrapper import (
+    CosyPoseWrapper,
+)
+from happypose.toolbox.inference.types import (
+    ObservationTensor,
+)
+
+import rospy
+from sensor_msgs.msg import Image, CameraInfo
+from geometry_msgs.msg import Pose, Point, Quaternion
+from pytorch3d import transforms
+
+
+def run_inference_from_realtime_camera(
+    camera_info: CameraInfo,
+    image: Image,
+    model_name: str,
+    dataset_to_use: str,
+) -> None:
+    try:
+        observation = ObservationTensor.from_numpy(image.data, None, camera_info.K)
+        CosyPose = CosyPoseWrapper(dataset_name=dataset_to_use, n_workers=8)
+        predictions = CosyPose.inference(observation)
+        return predictions.poses
+    except AttributeError as err:
+        print("rien trouvé")
+        return []
+
+
+if __name__ == "__main__":
+    dataset = rospy.get_param("~dataset", "ycbv")
+    model = rospy.get_param("~model", "megapose-1.0-RGB-multi-hypothesis")
+
+    rospy.init_node("caemra_listener", anonymous=True)
+    cam_info = rospy.wait_for_message("/camera/color/camera_info", CameraInfo)
+
+    image = rospy.wait_for_message("/camera/color/image_raw", Image)
+    inference = run_inference_from_realtime_camera(cam_info, image, model, dataset)
+    for i, pose in enumerate(inference):
+        quat = transforms.matrix_to_quaternion(pose[:3, :3])
+        pose = Pose(
+            position=Point(pose[0, 3], pose[1, 3], pose[2, 3]),
+            orientation=Quaternion(quat[0], quat[1], quat[2], quat[3]),
+        )
+        pub = rospy.Publisher(f"tf/happypose_object_{i}", Pose, queue_size=10)
+        pub.publish(pose)
+    if inference == []:
+        pose = Pose(
+            position=Point(0, 0, 0),
+            orientation=Quaternion(1, 0, 0, 0),
+        )
+        pub = rospy.Publisher("tf/happypose_object", Pose, queue_size=10)
+        pub.publish(pose)