修改接口名称

HiveCoreR1/src/robot_vision/vision_detect/vision_detect/crossboard_detect.py

@@ -1,6 +1,4 @@
-import os
 from collections import defaultdict
-import time
 
 import cv2
 import open3d as o3d
@@ -8,15 +6,13 @@ import numpy as np
 import transforms3d as tfs
 from cv_bridge import CvBridge
 
-from vision_detect.ultralytics import YOLO
-
 import rclpy
 from rclpy.node import Node
 from message_filters import ApproximateTimeSynchronizer, Subscriber
 
 from sensor_msgs.msg import Image, CameraInfo
 from geometry_msgs.msg import Pose, Point, Quaternion
-from vision_interfaces.msg import PoseWithID, PoseWithIDArray
+from vision_interfaces.msg import PoseClassAndID, PoseArrayClassAndID
 
 
 def get_map(K, D, camera_size):
@@ -238,7 +234,7 @@ class DetectNode(Node):
 
     def _init_publisher(self):
         """init_publisher"""
-        self.pub_pose_list = self.create_publisher(PoseWithIDArray, '/pose/cv_detect_pose', 10)
+        self.pub_pose_list = self.create_publisher(PoseArrayClassAndID, '/pose/cv_detect_pose', 10)
 
         if self.output_boxes or self.output_masks:
             self.pub_detect_image = self.create_publisher(Image, '/image/detect_image', 10)
@@ -299,10 +295,10 @@ class DetectNode(Node):
             self.pub_detect_image.publish(img)
 
         if pose_dict is not None:
-            pose_list_all = PoseWithIDArray()
+            pose_list_all = PoseArrayClassAndID()
             for (class_id, class_name), pose_list in pose_dict.items():
                 pose_list_all.objects.append(
-                    PoseWithID(
+                    PoseClassAndID(
                         class_name = class_name,
                         class_id = class_id,
                         pose_list = pose_list

HiveCoreR1/src/robot_vision/vision_detect/vision_detect/detect.py

@@ -17,7 +17,7 @@ from rclpy.node import Node
 
 from sensor_msgs.msg import Image
 from geometry_msgs.msg import Pose, Point, Quaternion
-from vision_interfaces.msg import PoseWithID
+from vision_interfaces.msg import PoseClassAndID
 from img_dev.msg import ImgMsg
 from vision_interfaces.srv import VisionObjectRecognition
 
@@ -273,6 +273,9 @@ class DetectNode(Node):
             ]
 
     def _service_callback(self, request, response):
+        response.header.stamp = self.get_clock().now().to_msg()
+        response.header.frame_id = "camera_detect"
+
         with self.lock:
             if request.camera_position in self.camera_data:
                 color_img_ros, depth_img_ros, self.K, D = self.camera_data[request.camera_position]
@@ -282,7 +285,7 @@ class DetectNode(Node):
                 response.objects = []
 
                 return response
-
+        
         self.camera_size = [color_img_ros.width, color_img_ros.height]
         
         color_img_cv = self.cv_bridge.imgmsg_to_cv2(color_img_ros, "bgr8")
@@ -304,7 +307,7 @@ class DetectNode(Node):
             response.success = True
             for (class_id, class_name), pose_list in pose_dict.items():
                 response.objects.append(
-                    PoseWithID(
+                    PoseClassAndID(
                         class_name = class_name,
                         class_id = class_id,
                         pose_list = pose_list

HiveCoreR1/src/robot_vision/vision_detect/vision_detect/hand_eye_calibration.py

@@ -11,7 +11,7 @@ from rclpy.parameter import Parameter
 
 from tf2_ros import Buffer
 from tf2_msgs.msg import TFMessage
-from vision_interfaces.msg import PoseWithIDArray
+from vision_interfaces.msg import PoseArrayClassAndID
 from message_filters import ApproximateTimeSynchronizer, Subscriber
 
 
@@ -185,7 +185,7 @@ class Calibration(Node):
 
     def _init_sub(self):
         self.sub_hand_pose = Subscriber(self, TFMessage, '/tf')
-        self.sub_camera_pose = Subscriber(self, PoseWithIDArray, '/pose/cv_detect_pose')
+        self.sub_camera_pose = Subscriber(self, PoseArrayClassAndID, '/pose/cv_detect_pose')
 
         self.sync_subscriber = ApproximateTimeSynchronizer(
             [self.sub_hand_pose, self.sub_camera_pose],

HiveCoreR1/src/robot_vision/vision_detect/vision_detect/service_client.py

@@ -1,13 +1,13 @@
 import rclpy
 from rclpy.node import Node
 
-from vision_interfaces.srv import PoseArray
+from vision_interfaces.srv import VisionObjectRecognition
 
 
 class Client(Node):
     def __init__(self, name):
         super().__init__(name)
-        self.client_ = self.create_client(PoseArray, "/get_object_pose_list",)
+        self.client_ = self.create_client(VisionObjectRecognition, "/get_object_pose_list",)
         self.timer_ = self.create_timer(2.0, self._timer_callback)
     
     def _timer_callback(self):
@@ -16,7 +16,7 @@ class Client(Node):
     def send_request(self, camera_position):
         while not self.client_.wait_for_service(timeout_sec=1.0):
             self.get_logger().info('service not available, waiting again...')
-        request = PoseArray.Request()
+        request = VisionObjectRecognition.Request()
         request.camera_position = camera_position
         self.client_.call_async(request).add_done_callback(self._result_callback)
     

HiveCoreR1/src/robot_vision/vision_detect/vision_detect/sub_pose.py

@@ -1,13 +1,13 @@
 import rclpy
 from rclpy.node import Node
 
-from vision_interfaces.msg import PoseWithIDArray
+from vision_interfaces.msg import PoseArrayClassAndID
 
 
 class SubPose(Node):
     def __init__(self, name):
         super().__init__(name)
-        self.sub = self.create_subscription(PoseWithIDArray, '/pose/cv_detect_pose', self.pose_callback, 10)
+        self.sub = self.create_subscription(PoseArrayClassAndID, '/pose/cv_detect_pose', self.pose_callback, 10)
 
     def pose_callback(self, msg):
         pose_dict = {}

HiveCoreR1/src/robot_vision/vision_interfaces/CMakeLists.txt

@@ -14,8 +14,8 @@ find_package(geometry_msgs REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-	"msg/PoseWithID.msg"
-	"msg/PoseWithIDArray.msg"
+	"msg/PoseClassAndID.msg"
+	"msg/PoseArrayClassAndID.msg"
   "srv/VisionObjectRecognition.srv"
 	DEPENDENCIES geometry_msgs
  )

HiveCoreR1/src/robot_vision/vision_interfaces/srv/VisionObjectRecognition.srv

@@ -2,6 +2,7 @@ string camera_position
 
 ---
 
+std_msgs/Header header
 string info
 bool success
 vision_interfaces/PoseWithID[] objects