fix service node

scripts/happypose_with_camera.py

@@ -10,6 +10,7 @@ from happypose.toolbox.inference.types import (
 
 import rospy
 import tf
+import numpy as np
 from ros_cosypose.srv import Inference, InferenceResponse
 from sensor_msgs.msg import Image, CameraInfo
 from std_msgs.msg import Int32MultiArray
@@ -24,14 +25,16 @@ def run_inference_from_realtime_camera(
     dataset_to_use: str,
 ) -> None:
     try:
-        observation = ObservationTensor.from_numpy(image.data, None, camera_info.K)
+        data = np.frombuffer(image.data, dtype=np.uint8)
+        data = np.reshape(data, (720, 1280, 3))
+        K = np.reshape(camera_info.K, (3, 3))
+        observation = ObservationTensor.from_numpy(data, None, K)
         CosyPose = CosyPoseWrapper(dataset_name=dataset_to_use, n_workers=8)
         predictions = CosyPose.inference(observation)
-        rospy.loginfo()
         return predictions.poses, predictions.infos
     except AttributeError as err:
         rospy.loginfo("rien trouvé")
-        return [], []
+        return None, None
 
 
 def retrieve_parameters_and_run_inference():
@@ -51,7 +54,7 @@ def convert_list_to_ros_msg(list):
 def get_nb_objects_in_image(req):
     inference, inference_infos = retrieve_parameters_and_run_inference()
     nb_objects_in_image = [0 for _ in range(31)]
-    if inference_infos == []:
+    if inference is None:
         return convert_list_to_ros_msg(nb_objects_in_image)
     labels = inference_infos["label"]
     for idx, pose in enumerate(inference):
@@ -60,6 +63,7 @@ def get_nb_objects_in_image(req):
             raise RuntimeError(
                 "label doesn't end with tless object index : ", object_number
             )
+
         object_number = int(object_number)
         nb_objects_in_image[object_number] += 1
         quat = transforms.matrix_to_quaternion(pose[:3, :3])
@@ -68,7 +72,7 @@ def get_nb_objects_in_image(req):
             (pose[0, 3], pose[1, 3], pose[2, 3]),
             (quat[0], quat[1], quat[2], quat[3]),
             rospy.Time.now(),
-            f"tf/happypose_object_{object_number}_{nb_objects_in_image[object_number]}",
+            f"tf/object_tless_{object_number}_{nb_objects_in_image[object_number]}",
             "camera_color_optical_frame",
         )
     return convert_list_to_ros_msg(nb_objects_in_image)