Add new files

camera_kl520/Scan_kl520.py

@@ -0,0 +1,77 @@
+# ******************************************************************************
+#  Copyright (c) 2021-2022. Kneron Inc. All rights reserved.                   *
+# ******************************************************************************
+#! /usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import os
+import sys
+import kp
+
+PWD = os.path.dirname(os.path.abspath(__file__))
+sys.path.insert(1, os.path.join(PWD, '..'))
+
+class ScanForTesting(Node):
+    def __init__(self, name):
+        super().__init__(name)
+
+        print('scanning kneron devices ...')
+
+        device_list = kp.core.scan_devices()
+
+        print('number of Kneron devices found: {}'.format(device_list.device_descriptor_number))
+
+        if device_list.device_descriptor_number == 0:
+            exit(0)
+
+        print('listing devices infomation as follows:')
+
+        for idx, device_descriptor in enumerate(device_list.device_descriptor_list):
+            print()
+            print('[{}] USB scan index: \'{}\''.format(idx, idx))
+            print('[{}] USB port ID: \'{}\''.format(idx, device_descriptor.usb_port_id))
+            print('[{}] Product ID: \'0x{:X} {}\''.format(idx,
+                                                        device_descriptor.product_id,
+                                                        self.get_product_name(product_id=device_descriptor.product_id)))
+
+            print('[{}] USB link speed: \'{}\''.format(idx, device_descriptor.link_speed))
+            print('[{}] USB port path: \'{}\''.format(idx, device_descriptor.usb_port_path))
+            print('[{}] KN number: \'0x{:X}\''.format(idx, device_descriptor.kn_number))
+            print('[{}] Connectable: \'{}\''.format(idx, device_descriptor.is_connectable))
+            print('[{}] Firmware: \'{}\''.format(idx, device_descriptor.firmware))
+
+
+    
+
+
+    def get_product_name(self, product_id: int) -> str:
+        if product_id == kp.ProductId.KP_DEVICE_KL520.value:
+            return '(KL520)'
+        elif product_id == kp.ProductId.KP_DEVICE_KL720.value:
+            return '(KL720)'
+        elif product_id == 0x200:
+            return '(KL720) (Please update firmware by Kneron DFUT)'
+        elif product_id == kp.ProductId.KP_DEVICE_KL630.value:
+            return '(KL630)'
+        elif product_id == kp.ProductId.KP_DEVICE_KL730.value:
+            return '(KL730)'
+        elif product_id == kp.ProductId.KP_DEVICE_KL830.value:
+            return '(KL830)'
+        else:
+            return '(Unknown Device)'
+        
+
+
+def main(args=None):
+        rclpy.init(args=args)
+        node = ScanForTesting("Testing_kl_520")
+        rclpy.spin_once(node)
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
+
+
+        

camera_kl520/camera_yolo5l_pub.py

@@ -0,0 +1,133 @@
+#! /usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from vision_msgs.msg import Detection2D, Detection2DArray, BoundingBox2D, ObjectHypothesisWithPose
+from geometry_msgs.msg import Pose2D
+from cv_bridge import CvBridge
+
+import cv2
+import torch
+
+class YoloInferenceNode_pub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        
+        # Create publishers for the annotated image and detection results
+        self.image_pub = self.create_publisher(Image, 'yolo/annotated', 10)
+        self.detections_pub = self.create_publisher(Detection2DArray, 'yolo/detections', 10)
+        
+        # Timer to call the callback every 0.2 seconds (approx. 5 FPS)
+        self.timer = self.create_timer(0.2, self.timer_callback)
+        
+        # CvBridge for converting between ROS Image messages and OpenCV images
+        self.bridge = CvBridge()
+        
+        # Open the camera using the V4L2 backend (suitable for WSL2)
+        self.cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
+        if not self.cap.isOpened():
+            self.get_logger().error("Unable to open camera")
+            rclpy.shutdown()
+        
+        # Set the camera format, resolution, and FPS
+        fourcc = cv2.VideoWriter_fourcc(*'MJPG')
+        self.cap.set(cv2.CAP_PROP_FOURCC, fourcc)
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+        self.cap.set(cv2.CAP_PROP_FPS, 30)
+        
+        self.get_logger().info("Loading YOLOv5 model...")
+        self.model = torch.hub.load('ultralytics/yolov5', 'yolov5l', pretrained=True)
+   
+        self.model.to('cuda')
+        self.model.eval()
+        self.get_logger().info("YOLOv5 model loaded.")
+    
+    def timer_callback(self):
+        ret, frame = self.cap.read()
+        if not ret:
+            self.get_logger().error("Failed to read image")
+            return
+        
+        # Convert BGR image to RGB
+        img_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+        
+        # Run inference
+        results = self.model(img_rgb)
+        
+        #---------------------------------------------------------
+        
+        # Render detections and get the annotated image (returned in RGB)
+        annotated_frame = results.render()[0]
+        # Convert annotated image back to BGR for cv2 display and ROS Image (bgr8)
+        annotated_frame = cv2.cvtColor(annotated_frame, cv2.COLOR_RGB2BGR)
+        
+        # Display the image
+        cv2.imshow("YOLOv5 Inference", annotated_frame)
+        cv2.waitKey(1)
+        
+        # Convert annotated_frame to a ROS Image message and publish it
+        img_msg = self.bridge.cv2_to_imgmsg(annotated_frame, encoding='bgr8')
+        header = img_msg.header
+        header.stamp = self.get_clock().now().to_msg()
+        header.frame_id = "C270_WEBCAM"
+        self.image_pub.publish(img_msg)
+        
+        # Parse YOLOv5 detection results with the format: [x1, y1, x2, y2, confidence, class]
+        detections = results.xyxy[0]
+        
+        # Create a Detection2DArray message
+        detection_array = Detection2DArray()
+        detection_array.header = header
+        
+        # Process each detection bounding box
+        for det in detections:
+            x1, y1, x2, y2, conf, cls = det.tolist()
+            detection = Detection2D()
+            detection.header = header
+            
+            # Create a BoundingBox2D and set its center and dimensions
+            bbox = BoundingBox2D()
+            bbox.center.position.x = (x1 + x2) / 2.0
+            bbox.center.position.y = (y1 + y2) / 2.0
+            bbox.center.theta = 0.0
+
+            bbox.size_x = x2 - x1
+            bbox.size_y = y2 - y1
+            detection.bbox = bbox
+            
+            # Add class and confidence to ObjectHypothesisWithPose
+            hypothesis = ObjectHypothesisWithPose()
+            hypothesis.hypothesis.class_id = str(int(cls))  # Class represented as an integer string
+            hypothesis.hypothesis.score = conf
+
+            # Append the hypothesis to the detection results
+            detection.results.append(hypothesis)
+            
+            # Append the detection to the detection array
+            detection_array.detections.append(detection)
+        
+        # Publish the detection array so that other ROS2 nodes can subscribe to it
+        self.detections_pub.publish(detection_array)
+        detections = results.xyxy[0]  # Each row represents one detection
+
+        for i, det in enumerate(detections):
+            x1, y1, x2, y2, conf, cls = det.tolist()
+            self.get_logger().info(
+                f"object class {cls}, Confidence: {conf:.2f}"
+            )
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = YoloInferenceNode_pub("yolo_inference_pub")
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("Shutting down...")
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

camera_kl520/camera_yolo5l_sub.py

@@ -0,0 +1,50 @@
+#! /usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from vision_msgs.msg import Detection2D, Detection2DArray, BoundingBox2D, ObjectHypothesisWithPose
+from geometry_msgs.msg import Pose2D
+from cv_bridge import CvBridge
+
+class YoloInferenceNode_pub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        # self.image_sub = self.create_subscription(Image, 'yolo/annotated',self.listener_callback, 10)
+        self.detections_sub = self.create_subscription(Detection2DArray, 'yolo/detections', self.listener_callback, 10)
+
+    def listener_callback(self, detection_array_msg):
+    # Check if there are any detections at all
+        if not detection_array_msg.detections:
+            self.get_logger().warn("No detections found in the received message.")
+            return
+
+        # Loop through each Detection2D message in the detection array
+        for det_index, detection in enumerate(detection_array_msg.detections):
+            # Check if the detection contains any results/hypotheses
+            if not detection.results:
+                self.get_logger().warn(f"Detection {det_index} has no results.")
+                continue
+
+            # Process each hypothesis for this detection
+            for hyp_index, hypothesis_wrapper in enumerate(detection.results):
+                # Accessing the nested hypothesis fields
+                class_id = hypothesis_wrapper.hypothesis.class_id
+                score = hypothesis_wrapper.hypothesis.score
+                self.get_logger().info(
+                    f"object class {class_id}, Confidence: {score:.2f}"
+                )
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = YoloInferenceNode_pub("yolo_inference_sub")
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("Shutting down...")
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

camera_kl520/camera_yolo5v_kl520_pub.py

@@ -0,0 +1,305 @@
+# ******************************************************************************
+#  Copyright (c) 2021-2022. Kneron Inc. All rights reserved.                   *
+# ******************************************************************************
+#! /usr/bin/env python3
+
+
+import rclpy
+from rclpy.node import Node
+# from std_msgs.msg import String
+from ament_index_python.packages import get_package_share_directory
+from vision_msgs.msg import Detection2D, Detection2DArray, BoundingBox2D, ObjectHypothesisWithPose
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+
+import os
+import cv2
+
+# current file directory pwd
+# PWD = os.path.dirname(os.path.abspath(__file__))
+# sys.path.insert(1, os.path.join(PWD, '..'))
+
+pkg_name = 'camera_kl520'
+pkg_share = get_package_share_directory(pkg_name)
+
+from utils.ExampleHelper import get_device_usb_speed_by_port_id
+from utils.ExamplePostProcess import post_process_yolo_v5
+import kp
+import cv2
+
+# firmware sources
+SCPU_FW_PATH = os.path.join(pkg_share, 'res', 'firmware', 'KL520', 'fw_scpu.bin')
+NCPU_FW_PATH = os.path.join(pkg_share, 'res', 'firmware', 'KL520', 'fw_ncpu.bin')
+
+# model and image file path
+MODEL_FILE_PATH = os.path.join(pkg_share, 'res', 'models', 'KL520', 'yolov5-noupsample_w640h640_kn-model-zoo', 'kl520_20005_yolov5-noupsample_w640h640.nef')
+IMAGE_FILE_PATH = os.path.join(pkg_share, 'res', 'images', 'testing_920x720.jpg')
+
+class ImageClassifierNode_kl520_pub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+
+        self.count = 0
+        self.declare_parameter('port_id', 0)
+        self.declare_parameter('model', MODEL_FILE_PATH)
+        self.declare_parameter('img', IMAGE_FILE_PATH)
+        
+        self.usb_port_id   = self.get_parameter('port_id').value
+        self.model_path    = self.get_parameter('model').value
+        self.image_file_path    = self.get_parameter('img').value
+
+        # Scan the device, get necessary parameters 
+        self.ScanProduct()
+
+        self.image_pub = self.create_publisher(Image, 'camera_yolov5_kl520/annotated', 10)
+        self.detections_pub = self.create_publisher(Detection2DArray, 'camera_yolov5_kl520/detections', 10)
+        
+        # Prepare Images
+        self.Prepare_image()
+
+        # Create directory for images
+        images_dir = os.path.join(pkg_share, "images")
+        os.makedirs(images_dir, exist_ok=True)
+
+                
+        
+        print('[Starting Inference Work]')
+        self.timer = self.create_timer(0.2, self.Inference) # 5fps
+
+        
+
+    def ScanProduct(self):
+        """
+        check device USB speed (Recommend run KL520 at high speed)
+        """
+        try:
+            if kp.UsbSpeed.KP_USB_SPEED_HIGH != get_device_usb_speed_by_port_id(usb_port_id=self.usb_port_id):
+                print('\033[91m' + '[Warning] Device is not run at high speed.' + '\033[0m')
+        except Exception as exception:
+            print('Error: check device USB speed fail, port ID = \'{}\', error msg: [{}]'.format(self.usb_port_id,
+                                                                                                str(exception)))
+            exit(0)
+
+        """
+        connect the device
+        """
+        try:
+            print('[Connect Device]')
+            self.device_group = kp.core.connect_devices(usb_port_ids=[self.usb_port_id])
+            print(' - Success')
+        except kp.ApiKPException as exception:
+            print('Error: connect device fail, port ID = \'{}\', error msg: [{}]'.format(self.usb_port_id,
+                                                                                        str(exception)))
+            exit(0)
+
+        """
+        setting timeout of the usb communication with the device
+        """
+        print('[Set Device Timeout]')
+        kp.core.set_timeout(device_group=self.device_group, milliseconds=5000)
+        print(' - Success')
+
+        """
+        upload firmware to device
+        """
+        try:
+            print('[Upload Firmware]')
+            kp.core.load_firmware_from_file(device_group=self.device_group,
+                                            scpu_fw_path=SCPU_FW_PATH,
+                                            ncpu_fw_path=NCPU_FW_PATH)
+            print(' - Success')
+        except kp.ApiKPException as exception:
+            print('Error: upload firmware failed, error = \'{}\''.format(str(exception)))
+            exit(0)
+
+        """
+        upload model to device
+        """
+        try:
+            print('[Upload Model]')
+            self.model_nef_descriptor = kp.core.load_model_from_file(device_group=self.device_group,
+                                                                file_path=self.model_path)
+            print(' - Success')
+        except kp.ApiKPException as exception:
+            print('Error: upload model failed, error = \'{}\''.format(str(exception)))
+            exit(0)
+            
+    def Prepare_image(self):
+        """
+        Set the camera format, resolution, and FPS
+        """
+
+        print("[Configuring Image]")
+        
+        self.bridge = CvBridge()
+
+        self.cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
+        if not self.cap.isOpened():
+            self.get_logger().error("Unable to open camera")
+            rclpy.shutdown()
+        
+        
+        fourcc = cv2.VideoWriter_fourcc(*'MJPG')
+        self.cap.set(cv2.CAP_PROP_FOURCC, fourcc)
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 640)
+        self.cap.set(cv2.CAP_PROP_FPS, 5)
+       
+
+    def Inference(self):
+        """
+        starting inference work
+        """
+        
+        print(' - ', end='')
+
+        self.ret, self.frame = self.cap.read()
+        if not self.ret:
+            self.get_logger().error("Falied to read image")
+            return 
+        
+        # self.img = cv2.imread(filename=self.image_file_path)
+        # self.img = cv2.resize(self.frame, (640, 640)) # modify the size of the image to fit the model 
+        self.img = cv2.resize(self.frame, (960, 720)) # modify the size of the image to fit the model 
+        self.img_bgr565 = cv2.cvtColor(src=self.img, code=cv2.COLOR_BGR2BGR565)
+
+        """
+        prepare generic image inference input descriptor
+        """
+        self.generic_inference_input_descriptor = kp.GenericImageInferenceDescriptor(
+            model_id=self.model_nef_descriptor.models[0].id,
+            inference_number=0,
+            input_node_image_list=[
+                kp.GenericInputNodeImage(
+                    image=self.img_bgr565,
+                    image_format=kp.ImageFormat.KP_IMAGE_FORMAT_RGB565,
+                    resize_mode=kp.ResizeMode.KP_RESIZE_ENABLE,
+                    padding_mode=kp.PaddingMode.KP_PADDING_CORNER,
+                    normalize_mode=kp.NormalizeMode.KP_NORMALIZE_KNERON
+                )
+            ]
+        )
+
+        try:
+            kp.inference.generic_image_inference_send(device_group=self.device_group,
+                                                    generic_inference_input_descriptor=self.generic_inference_input_descriptor)
+
+            self.generic_raw_result = kp.inference.generic_image_inference_receive(device_group=self.device_group)
+        except kp.ApiKPException as exception:
+            print(' - Error: inference failed, error = {}'.format(exception))
+            exit(0)
+
+            print('.', end='', flush=True)
+        print()
+
+        """
+        retrieve inference node output 
+        """
+        print('[Retrieve Inference Node Output ]')
+        self.inf_node_output_list = []
+        for node_idx in range(self.generic_raw_result.header.num_output_node):
+            inference_float_node_output = kp.inference.generic_inference_retrieve_float_node(
+                node_idx=node_idx,
+                generic_raw_result=self.generic_raw_result,
+                channels_ordering=kp.ChannelOrdering.KP_CHANNEL_ORDERING_CHW
+            )
+            self.inf_node_output_list.append(inference_float_node_output)
+
+        print(' - Success')
+
+        self.yolo_result = post_process_yolo_v5(inference_float_node_output_list=self.inf_node_output_list,
+                                        hardware_preproc_info=self.generic_raw_result.header.hw_pre_proc_info_list[0],
+                                        thresh_value=0.2)
+        
+        # # msg = String()
+        # coords = []
+        # print(" - Bounding boxes info (xmin,ymin,xmax,ymax, confidence, class):")
+        # print(" - Bounding boxes info (xmin,ymin,xmax,ymax, confidence, class):")
+        output_img_name = f'output_{self.count}_{os.path.basename(self.image_file_path)}'
+        self.count += 1
+        print(f"stored image {output_img_name}")
+
+        # for box in self.yolo_result.box_list:
+        #     b = 100 + (25 * box.class_num) % 156
+        #     g = 100 + (80 + 40 * box.class_num) % 156
+        #     r = 100 + (120 + 60 * box.class_num) % 156
+        #     color = (b, g, r)
+
+        #     cv2.rectangle(img=self.img,
+        #                 pt1=(int(box.x1), int(box.y1)),
+        #                 pt2=(int(box.x2), int(box.y2)),
+        #                 color=color,
+        #                 thickness=2)
+        #     coords.append(f"[{int(box.x1)}, {int(box.y1)}, {int(box.x2)}, {int(box.y2)}], confidence : {box.score}, class : {box.class_num}\n")
+       
+        # msg.data = "".join(coords)
+        # self.image_pub.publish(msg)
+        # self.get_logger().info(f"Published: \n{msg.data}")
+        annotated = self.img.copy()
+        det_array = Detection2DArray()
+        det_array.header.stamp = self.get_clock().now().to_msg()
+        det_array.header.frame_id = "C270_WEBCAM"
+
+        for box in self.yolo_result.box_list:
+            b = 100 + (25 * box.class_num) % 156
+            g = 100 + (80 + 40 * box.class_num) % 156
+            r = 100 + (120 + 60 * box.class_num) % 156
+            color = (b, g, r)
+            cv2.rectangle(img=annotated,
+                          pt1=(int(box.x1), int(box.y1)),
+                          pt2=(int(box.x2), int(box.y2)),
+                          color=color,
+                          thickness=2)
+            
+            detection = Detection2D()
+            detection.header = det_array.header
+
+            # Create a BoundingBox2D and set its center and dimensions
+            bbox = BoundingBox2D()
+            bbox.center.position.x = (box.x1 + box.x2) / 2.0
+            bbox.center.position.y = (box.y1 + box.y2) / 2.0
+            bbox.center.theta = 0.0
+
+            bbox.size_x = float(box.x2 - box.x1)
+            bbox.size_y = float(box.y2 - box.y1)
+            detection.bbox = bbox
+
+            # Add class and confidence to ObjectHypothesisWithPose
+            hypo = ObjectHypothesisWithPose()
+            hypo.hypothesis.class_id = str(int(box.class_num))
+            hypo.hypothesis.score = box.score
+
+            # Append the hypothesis to the detection results
+            detection.results.append(hypo)
+
+            # Append the detection to the detection array
+            det_array.detections.append(detection)
+
+            self.get_logger().info(
+                f"class: {box.class_num}, conf: {box.score:.2f}"
+            )
+        
+        # Publish the detection array so that other ROS2 nodes can subscribe to it
+        img_msg = self.bridge.cv2_to_imgmsg(annotated, encoding='bgr8')
+        img_msg.header = det_array.header          # under same time line
+        self.image_pub.publish(img_msg)
+        self.detections_pub.publish(det_array)
+        # Store the image for debugging.
+        
+        cv2.imwrite(os.path.join(pkg_share, "images", './{}'.format(output_img_name)), img=annotated)
+        
+
+
+
+def main(args = None):
+    rclpy.init(args = args)
+    node = ImageClassifierNode_kl520_pub("camera_yolov5_kl520_pub")
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("shutting down...")
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

camera_kl520/camera_yolo5v_kl520_sub.py

@@ -0,0 +1,91 @@
+#! /usr/bin/env python3
+
+# from std_msgs.msg import String
+# from sensor_msgs.msg import Image
+import rclpy
+import rclpy.executors
+from rclpy.node import Node
+from vision_msgs.msg import Detection2DArray
+# We only need Detection2DArray since we packaged everything inside this 2D Array in the publisher Node.
+
+import sys, os
+from ament_index_python.packages import get_package_prefix
+
+prefix = get_package_prefix('ur_robot_driver')
+scripts_dir = os.path.join(prefix, 'lib', 'ur_robot_driver')  # where ur3e_actions.py located.
+sys.path.append(scripts_dir)
+
+from ur3e_actions import JTCClient
+
+
+
+
+class ImageClassifierNode_sub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        self.arm_busy = False
+        self.arm = JTCClient("ur3e_traj_client")
+        
+        def wrap_result_cb():
+            self.arm_busy = False # unlock
+            self.get_logger().info("arm done → resume detection")
+
+        self.arm.result_cb = wrap_result_cb
+    
+        self.sub = self.create_subscription(Detection2DArray, 'camera_yolov5_kl520/detections',self.listener_callback, 10)
+       
+    def listener_callback(self, detection_array_msg):
+        # msg = msg.data
+        # self.get_logger().info(f"I received msg \n{msg}")
+
+        if self.arm_busy:
+            print("Moving......")
+            return
+        
+        if not detection_array_msg.detections:
+            self.get_logger().info("No detections found in the received message.")
+            return
+        
+        # Loop through each Detection2D message in the detection array
+        print("-"*20)
+        for det_index, detection in enumerate(detection_array_msg.detections):
+            # Check if the detection contains any results/hypotheses
+            if not detection.results:
+                self.get_logger().warn(f"Detection {det_index} has no results.")
+                continue
+
+            # Process each hypothesis for this detection
+            for hyp_index, hypothesis_wrapper in enumerate(detection.results):
+                # Accessing the nested hypothesis fields
+                class_id = hypothesis_wrapper.hypothesis.class_id
+                score = hypothesis_wrapper.hypothesis.score
+                self.get_logger().info(
+                    f"object class {class_id}, Confidence: {score:.2f}"
+                )
+
+                if class_id == "0" and score > 0.5:
+                    self.arm_busy = True                           # lock it
+                    fut = self.arm.execute_trajectory('traj0')     # received result_future
+                    fut.add_done_callback(lambda _: setattr(self, "arm_busy", False))
+                    
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ImageClassifierNode_sub("camera_yolov5_kl520_sub")
+    arm_node = node.arm
+
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    executor.add_node(arm_node)
+
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        node.get_logger().info("Shutting down...")
+    node.destroy_node()
+    arm_node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+

camera_kl520/camera_yolo5v_kl720_pub.py

@@ -0,0 +1,279 @@
+# ******************************************************************************
+#  Copyright (c) 2021-2022. Kneron Inc. All rights reserved.                   *
+# ******************************************************************************
+import rclpy
+from rclpy.node import Node
+# from std_msgs.msg import String
+from ament_index_python.packages import get_package_share_directory
+from vision_msgs.msg import Detection2D, Detection2DArray, BoundingBox2D, ObjectHypothesisWithPose
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+
+import os
+import sys
+import argparse
+
+# PWD = os.path.dirname(os.path.abspath(__file__))
+# sys.path.insert(1, os.path.join(PWD, '..'))
+# sys.path.insert(1, os.path.join(PWD, '../example/'))
+
+pkg_name = 'camera_kl520'
+pkg_share = get_package_share_directory(pkg_name)
+
+from utils.ExampleHelper import get_device_usb_speed_by_port_id
+from utils.ExamplePostProcess import post_process_yolo_v5
+import kp
+import cv2
+
+# model and image file path
+MODEL_FILE_PATH = os.path.join(pkg_share, 'res', 'models', 'kl720', 'yolov5-noupsample_w640h640_kn-model-zoo/kl720_20005_yolov5-noupsample_w640h640.nef')
+IMAGE_FILE_PATH = os.path.join(pkg_share, 'res', 'images', 'kl720', 'people_talk_in_street_1500x1500.bmp')
+
+
+class ImageClassifierNode_kl720_pub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+
+        self.count = 0
+        self.declare_parameter('port_id', 0)
+        self.declare_parameter('model', MODEL_FILE_PATH)
+        self.declare_parameter('img', IMAGE_FILE_PATH)
+        
+        self.usb_port_id   = self.get_parameter('port_id').value
+        self.model_path    = self.get_parameter('model').value
+        self.image_file_path    = self.get_parameter('img').value
+
+        # Scan the device, get necessary parameters 
+        self.ScanProduct()
+
+        self.image_pub = self.create_publisher(Image, 'camera_yolov5_kl720/annotated', 10)
+        self.detections_pub = self.create_publisher(Detection2DArray, 'camera_yolov5_kl720/detections', 10)
+        
+        # Prepare Images
+        self.Prepare_image()
+
+        # Create directory for images
+        images_dir = os.path.join(pkg_share, "images")
+        os.makedirs(images_dir, exist_ok=True)
+                
+        
+        print('[Starting Inference Work]')
+        self.timer = self.create_timer(0.1, self.Inference) # 10fps
+
+    def ScanProduct(self):
+        """
+        check device USB speed (Recommend run KL720 at super speed)
+        """
+        try:
+            if kp.UsbSpeed.KP_USB_SPEED_SUPER != get_device_usb_speed_by_port_id(usb_port_id=self.usb_port_id):
+                print('\033[91m' + '[Warning] Device is not run at super speed.' + '\033[0m')
+        except Exception as exception:
+            print('Error: check device USB speed fail, port ID = \'{}\', error msg: [{}]'.format(self.usb_port_id,
+                                                                                                str(exception)))
+            exit(0)
+
+        """
+        connect the device
+        """
+        try:
+            print('[Connect Device]')
+            device_group = kp.core.connect_devices(usb_port_ids=[self.usb_port_id])
+            print(' - Success')
+        except kp.ApiKPException as exception:
+            print('Error: connect device fail, port ID = \'{}\', error msg: [{}]'.format(self.usb_port_id,
+                                                                                        str(exception)))
+            exit(0)
+
+        """
+        setting timeout of the usb communication with the device
+        """
+        print('[Set Device Timeout]')
+        kp.core.set_timeout(device_group=device_group, milliseconds=5000)
+        print(' - Success')
+
+        """
+        upload model to device
+        """
+        try:
+            print('[Upload Model]')
+            model_nef_descriptor = kp.core.load_model_from_file(device_group=device_group,
+                                                                file_path=self.model_path)
+            print(' - Success')
+        except kp.ApiKPException as exception:
+            print('Error: upload model failed, error = \'{}\''.format(str(exception)))
+            exit(0)
+
+    def Prepare_image(self):
+        """
+        Set the camera format, resolution, and FPS
+        """
+
+        print("[Configuring Image]")
+        
+        self.bridge = CvBridge()
+
+        self.cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
+        if not self.cap.isOpened():
+            self.get_logger().error("Unable to open camera")
+            rclpy.shutdown()
+        
+        
+        fourcc = cv2.VideoWriter_fourcc(*'MJPG')
+        self.cap.set(cv2.CAP_PROP_FOURCC, fourcc)
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 640)
+        self.cap.set(cv2.CAP_PROP_FPS, 5)
+
+    def Inference(self):
+        """
+        starting inference work
+        """
+
+        print(' - ', end='')
+
+        self.ret, self.frame = self.cap.read()
+        if not self.ret:
+            self.get_logger().error("Falied to read image")
+            return 
+
+        self.img = cv2.resize(self.frame, (960, 720)) # modify the size of the image to fit the model 
+        self.img_bgr565 = cv2.cvtColor(src=self.img, code=cv2.COLOR_BGR2BGR565)
+
+        print(' - Success')
+
+        """
+        prepare generic image inference input descriptor
+        """
+        self.generic_inference_input_descriptor = kp.GenericImageInferenceDescriptor(
+            model_id=self.model_nef_descriptor.models[0].id,
+            inference_number=0,
+            input_node_image_list=[
+                kp.GenericInputNodeImage(
+                    image=self.img_bgr565,
+                    image_format=kp.ImageFormat.KP_IMAGE_FORMAT_RGB565,
+                    resize_mode=kp.ResizeMode.KP_RESIZE_ENABLE,
+                    padding_mode=kp.PaddingMode.KP_PADDING_CORNER,
+                    normalize_mode=kp.NormalizeMode.KP_NORMALIZE_KNERON
+                )
+            ]
+        )
+
+        try:
+            kp.inference.generic_image_inference_send(device_group=self.device_group,
+                                                      generic_inference_input_descriptor=self.generic_inference_input_descriptor)
+
+            self.generic_raw_result = kp.inference.generic_image_inference_receive(device_group=self.device_group)
+        except kp.ApiKPException as exception:
+            print(' - Error: inference failed, error = {}'.format(exception))
+            exit(0)
+
+            print('.', end='', flush=True)
+        print()
+
+        """
+        retrieve inference node output 
+        """
+        print('[Retrieve Inference Node Output ]')
+
+        self.inf_node_output_list = []
+        for node_idx in range(self.generic_raw_result.header.num_output_node):
+            inference_float_node_output = kp.inference.generic_inference_retrieve_float_node(
+                node_idx=node_idx,
+                generic_raw_result=self.generic_raw_result,
+                channels_ordering=kp.ChannelOrdering.KP_CHANNEL_ORDERING_CHW
+            )
+            self.inf_node_output_list.append(inference_float_node_output)
+
+        print(' - Success')
+
+        self.yolo_result = post_process_yolo_v5(inference_float_node_output_list=self.inf_node_output_list,
+                                        hardware_preproc_info=self.generic_raw_result.header.hw_pre_proc_info_list[0],
+                                        thresh_value=0.2)
+
+        output_img_name = f'output_{self.count}_{os.path.basename(self.image_file_path)}'
+        self.count += 1
+        print(f"stored image {output_img_name}")
+
+        # for box in self.yolo_result.box_list:
+        #     b = 100 + (25 * box.class_num) % 156
+        #     g = 100 + (80 + 40 * box.class_num) % 156
+        #     r = 100 + (120 + 60 * box.class_num) % 156
+        #     color = (b, g, r)
+
+        #     cv2.rectangle(img=self.img,
+        #                 pt1=(int(box.x1), int(box.y1)),
+        #                 pt2=(int(box.x2), int(box.y2)),
+        #                 color=color,
+        #                 thickness=2)
+        #     coords.append(f"[{int(box.x1)}, {int(box.y1)}, {int(box.x2)}, {int(box.y2)}], confidence : {box.score}, class : {box.class_num}\n")
+       
+        # msg.data = "".join(coords)
+        # self.image_pub.publish(msg)
+        # self.get_logger().info(f"Published: \n{msg.data}")
+        annotated = self.img.copy()
+        det_array = Detection2DArray()
+        det_array.header.stamp = self.get_clock().now().to_msg()
+        det_array.header.frame_id = "C270_WEBCAM"
+
+        for box in self.yolo_result.box_list:
+            b = 100 + (25 * box.class_num) % 156
+            g = 100 + (80 + 40 * box.class_num) % 156
+            r = 100 + (120 + 60 * box.class_num) % 156
+            color = (b, g, r)
+            cv2.rectangle(img=annotated,
+                          pt1=(int(box.x1), int(box.y1)),
+                          pt2=(int(box.x2), int(box.y2)),
+                          color=color,
+                          thickness=2)
+            
+            detection = Detection2D()
+            detection.header = det_array.header
+
+            # Create a BoundingBox2D and set its center and dimensions
+            bbox = BoundingBox2D()
+            bbox.center.position.x = (box.x1 + box.x2) / 2.0
+            bbox.center.position.y = (box.y1 + box.y2) / 2.0
+            bbox.center.theta = 0.0
+
+            bbox.size_x = float(box.x2 - box.x1)
+            bbox.size_y = float(box.y2 - box.y1)
+            detection.bbox = bbox
+
+            # Add class and confidence to ObjectHypothesisWithPose
+            hypo = ObjectHypothesisWithPose()
+            hypo.hypothesis.class_id = str(int(box.class_num))
+            hypo.hypothesis.score = box.score
+
+            # Append the hypothesis to the detection results
+            detection.results.append(hypo)
+
+            # Append the detection to the detection array
+            det_array.detections.append(detection)
+
+            self.get_logger().info(
+                f"class: {box.class_num}, conf: {box.score:.2f}"
+            )
+        
+        # Publish the detection array so that other ROS2 nodes can subscribe to it
+        img_msg = self.bridge.cv2_to_imgmsg(annotated, encoding='bgr8')
+        img_msg.header = det_array.header          # under same time line
+        self.image_pub.publish(img_msg)
+        self.detections_pub.publish(det_array)
+        # Store the image for debugging.
+        cv2.imwrite(os.path.join(pkg_share, "images", './{}'.format(output_img_name)), img=annotated)
+        
+
+
+
+def main(args = None):
+    rclpy.init(args = args)
+    node = ImageClassifierNode_kl720_pub("camera_yolov5_kl720_pub")
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("shutting down...")
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

camera_kl520/camera_yolo5v_kl720_sub.py

@@ -0,0 +1,91 @@
+#! /usr/bin/env python3
+
+# from std_msgs.msg import String
+# from sensor_msgs.msg import Image
+import rclpy
+import rclpy.executors
+from rclpy.node import Node
+from vision_msgs.msg import Detection2DArray
+# We only need Detection2DArray since we packaged everything inside this 2D Array in the publisher Node.
+
+import sys, os
+from ament_index_python.packages import get_package_prefix
+
+prefix = get_package_prefix('ur_robot_driver')
+scripts_dir = os.path.join(prefix, 'lib', 'ur_robot_driver')  # where ur3e_actions.py located.
+sys.path.append(scripts_dir)
+
+from ur3e_actions import JTCClient
+
+
+
+
+class ImageClassifierNode_sub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        self.arm_busy = False
+        self.arm = JTCClient("ur3e_traj_client")
+        
+        def wrap_result_cb():
+            self.arm_busy = False # unlock
+            self.get_logger().info("arm done → resume detection")
+
+        self.arm.result_cb = wrap_result_cb
+    
+        self.sub = self.create_subscription(Detection2DArray, 'camera_yolov5_kl520/detections',self.listener_callback, 10)
+       
+    def listener_callback(self, detection_array_msg):
+        # msg = msg.data
+        # self.get_logger().info(f"I received msg \n{msg}")
+
+        if self.arm_busy:
+            print("Moving......")
+            return
+        
+        if not detection_array_msg.detections:
+            self.get_logger().info("No detections found in the received message.")
+            return
+        
+        # Loop through each Detection2D message in the detection array
+        print("-"*20)
+        for det_index, detection in enumerate(detection_array_msg.detections):
+            # Check if the detection contains any results/hypotheses
+            if not detection.results:
+                self.get_logger().warn(f"Detection {det_index} has no results.")
+                continue
+
+            # Process each hypothesis for this detection
+            for hyp_index, hypothesis_wrapper in enumerate(detection.results):
+                # Accessing the nested hypothesis fields
+                class_id = hypothesis_wrapper.hypothesis.class_id
+                score = hypothesis_wrapper.hypothesis.score
+                self.get_logger().info(
+                    f"object class {class_id}, Confidence: {score:.2f}"
+                )
+
+                if class_id == "0" and score > 0.5:
+                    self.arm_busy = True                           # lock it
+                    fut = self.arm.execute_trajectory('traj0')     # received result_future
+                    fut.add_done_callback(lambda _: setattr(self, "arm_busy", False))
+                    
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ImageClassifierNode_sub("camera_yolov5_kl520_sub")
+    arm_node = node.arm
+
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    executor.add_node(arm_node)
+
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        node.get_logger().info("Shutting down...")
+    node.destroy_node()
+    arm_node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+

camera_kl520/infos/coco.txt

@@ -0,0 +1,81 @@
+{0: u'__background__',
+ 1: u'person',
+ 2: u'bicycle',
+ 3: u'car',
+ 4: u'motorcycle',
+ 5: u'airplane',
+ 6: u'bus',
+ 7: u'train',
+ 8: u'truck',
+ 9: u'boat',
+ 10: u'traffic light',
+ 11: u'fire hydrant',
+ 12: u'stop sign',
+ 13: u'parking meter',
+ 14: u'bench',
+ 15: u'bird',
+ 16: u'cat',
+ 17: u'dog',
+ 18: u'horse',
+ 19: u'sheep',
+ 20: u'cow',
+ 21: u'elephant',
+ 22: u'bear',
+ 23: u'zebra',
+ 24: u'giraffe',
+ 25: u'backpack',
+ 26: u'umbrella',
+ 27: u'handbag',
+ 28: u'tie',
+ 29: u'suitcase',
+ 30: u'frisbee',
+ 31: u'skis',
+ 32: u'snowboard',
+ 33: u'sports ball',
+ 34: u'kite',
+ 35: u'baseball bat',
+ 36: u'baseball glove',
+ 37: u'skateboard',
+ 38: u'surfboard',
+ 39: u'tennis racket',
+ 40: u'bottle',
+ 41: u'wine glass',
+ 42: u'cup',
+ 43: u'fork',
+ 44: u'knife',
+ 45: u'spoon',
+ 46: u'bowl',
+ 47: u'banana',
+ 48: u'apple',
+ 49: u'sandwich',
+ 50: u'orange',
+ 51: u'broccoli',
+ 52: u'carrot',
+ 53: u'hot dog',
+ 54: u'pizza',
+ 55: u'donut',
+ 56: u'cake',
+ 57: u'chair',
+ 58: u'couch',
+ 59: u'potted plant',
+ 60: u'bed',
+ 61: u'dining table',
+ 62: u'toilet',
+ 63: u'tv',
+ 64: u'laptop',
+ 65: u'mouse',
+ 66: u'remote',
+ 67: u'keyboard',
+ 68: u'cell phone',
+ 69: u'microwave',
+ 70: u'oven',
+ 71: u'toaster',
+ 72: u'sink',
+ 73: u'refrigerator',
+ 74: u'book',
+ 75: u'clock',
+ 76: u'vase',
+ 77: u'scissors',
+ 78: u'teddy bear',
+ 79: u'hair drier',
+ 80: u'toothbrush'}

camera_kl520/object_detection.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from vision_msgs.msg import Detection2D, ObjectHypothesisWithPose, BoundingBox2D
+from cv_bridge import CvBridge, CvBridgeError
+from geometry_msgs.msg import Pose2D
+import cv2
+import torch ## we have it in our jack env, but not root, so it doesn't matter. 
+import torchvision
+from torchvision.models.detection import FasterRCNN_ResNet50_FPN_Weights
+
+
+class ObjectDetectionNode(Node):
+
+    def __init__(self, name):
+        super().__init__(name)
+        self.subscriptions = self.create_subscription(Image, 'image_raw', self.listener_callback, 10)
+        self.publishers = self.create_publisher(Image, 'detection_iamge', 10)
+        # Initializes the CvBridge for converting ROS images to OpenCV images.
+        self.bridge = CvBridge()
+        self.model = torchvision.models.detection.fasterrcnn_resnet50_fpn(FasterRCNN_ResNet50_FPN_Weights.DEFAULT)
+        self.model.eval()
+        self.coco_labels = {v: k for k, v in self.model.coco_labels.items()}
+        
+
+    def listener_callback(self, msg):
+        '''
+        Callback function triggered when a new image is received.
+        '''
+        try:
+            # Converts the ROS image message to an OpenCV image in BGR format.
+            cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+            # convert the image to a tensor.
+            transform = torchvision.transforms.Compose([torchvision.transforms.ToTensor()])
+            image_tensor = transform(cv_image)
+            outputs = self.model([image_tensor])[0]
+            for i, (box, score, label) in enumerate(zip(outputs['boxes'], outputs['scores'], outputs['labels'])):
+                if score >= 0.5:
+                    x1, y1, x2, y2 = box.int().tolist()
+                    label_name = self.coco_labels[label.item()]
+                    cv2.rectangle(cv_image, (x1, y1), (x2, y2), (0, 255, 0), 2)
+                    overlay = cv_image.copy()
+                    cv2.rectangle(overlay, (x1, y1), (x2, y2), (0, 255, 0), -1)
+                    alpha = 0.4
+                    cv_image = cv2.addWeighted(overlay, alpha, cv_image, 1 - alpha, 0)
+                    cv2.putText(cv_image, label_name, (x1, y1 - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2)
+            try:
+                detection_image = self.bridge.cv2_to_imgmsg(cv_image, 'bgr8')
+                self.publisher_.publish(detection_image)
+            except CvBridgeError as e:
+                self.get_logger().error('Failed to convert image: %s' % str(e))
+
+        except CvBridgeError as e:
+            self.get_logger().error('Failed to convert image: %s' % str(e))
+            return
+        
+def main(args=None):
+    rclpy.init(args = args)
+    detect = ObjectDetectionNode("nice")
+    rclpy.spin(detect)
+    detect.destroy_node()
+    detect.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+
+     
+    
+

camera_kl520/object_detection_kl520_pub.py

@@ -0,0 +1,214 @@
+# ******************************************************************************
+#  Copyright (c) 2021-2022. Kneron Inc. All rights reserved.                   *
+# ******************************************************************************
+#! /usr/bin/env python3
+
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from ament_index_python.packages import get_package_share_directory
+import os
+import sys
+
+# current file directory pwd
+# PWD = os.path.dirname(os.path.abspath(__file__))
+# sys.path.insert(1, os.path.join(PWD, '..'))
+
+pkg_name = 'camera_kl520'
+pkg_share = get_package_share_directory(pkg_name)
+
+from utils.ExampleHelper import get_device_usb_speed_by_port_id
+from utils.ExamplePostProcess import post_process_yolo_v5
+import kp
+import cv2
+
+# firmware sources
+SCPU_FW_PATH = os.path.join(pkg_share, 'res', 'firmware', 'KL520', 'fw_scpu.bin')
+NCPU_FW_PATH = os.path.join(pkg_share, 'res', 'firmware', 'KL520', 'fw_ncpu.bin')
+
+# model and image file path
+MODEL_FILE_PATH = os.path.join(pkg_share, 'res', 'models', 'KL520', 'yolov5-noupsample_w640h640_kn-model-zoo', 'kl520_20005_yolov5-noupsample_w640h640.nef')
+IMAGE_FILE_PATH = os.path.join(pkg_share, 'res', 'images', 'mason_640x640.jpg')
+
+class ImageClassifierNode_kl520_pub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+
+        self.declare_parameter('port_id', 0)
+        self.declare_parameter('model', MODEL_FILE_PATH)
+        self.declare_parameter('img', IMAGE_FILE_PATH)
+        
+        self.usb_port_id   = self.get_parameter('port_id').value
+        self.model_path    = self.get_parameter('model').value
+        self.image_file_path    = self.get_parameter('img').value
+
+        # Scan the device, get necessary parameters 
+        self.ScanProduct()
+        # Prepare Images
+        self.prepareImage()
+
+                
+        self.pub = self.create_publisher(String, 'kl520_objection_detection/result', 10)
+        self.timer = self.create_timer(1.0, self.Inference)
+
+        
+
+    def ScanProduct(self):
+        """
+        check device USB speed (Recommend run KL520 at high speed)
+        """
+        try:
+            if kp.UsbSpeed.KP_USB_SPEED_HIGH != get_device_usb_speed_by_port_id(usb_port_id=self.usb_port_id):
+                print('\033[91m' + '[Warning] Device is not run at high speed.' + '\033[0m')
+        except Exception as exception:
+            print('Error: check device USB speed fail, port ID = \'{}\', error msg: [{}]'.format(self.usb_port_id,
+                                                                                                str(exception)))
+            exit(0)
+
+        """
+        connect the device
+        """
+        try:
+            print('[Connect Device]')
+            self.device_group = kp.core.connect_devices(usb_port_ids=[self.usb_port_id])
+            print(' - Success')
+        except kp.ApiKPException as exception:
+            print('Error: connect device fail, port ID = \'{}\', error msg: [{}]'.format(self.usb_port_id,
+                                                                                        str(exception)))
+            exit(0)
+
+        """
+        setting timeout of the usb communication with the device
+        """
+        print('[Set Device Timeout]')
+        kp.core.set_timeout(device_group=self.device_group, milliseconds=5000)
+        print(' - Success')
+
+        """
+        upload firmware to device
+        """
+        try:
+            print('[Upload Firmware]')
+            kp.core.load_firmware_from_file(device_group=self.device_group,
+                                            scpu_fw_path=SCPU_FW_PATH,
+                                            ncpu_fw_path=NCPU_FW_PATH)
+            print(' - Success')
+        except kp.ApiKPException as exception:
+            print('Error: upload firmware failed, error = \'{}\''.format(str(exception)))
+            exit(0)
+
+        """
+        upload model to device
+        """
+        try:
+            print('[Upload Model]')
+            self.model_nef_descriptor = kp.core.load_model_from_file(device_group=self.device_group,
+                                                                file_path=self.model_path)
+            print(' - Success')
+        except kp.ApiKPException as exception:
+            print('Error: upload model failed, error = \'{}\''.format(str(exception)))
+            exit(0)
+            
+    def prepareImage(self):
+        """
+        prepare the image
+        """
+
+        print('[Read Image]')
+        self.img = cv2.imread(filename=self.image_file_path)
+        self.img_bgr565 = cv2.cvtColor(src=self.img, code=cv2.COLOR_BGR2BGR565)
+        print(' - Success')
+
+        """
+        prepare generic image inference input descriptor
+        """
+        self.generic_inference_input_descriptor = kp.GenericImageInferenceDescriptor(
+            model_id=self.model_nef_descriptor.models[0].id,
+            inference_number=0,
+            input_node_image_list=[
+                kp.GenericInputNodeImage(
+                    image=self.img_bgr565,
+                    image_format=kp.ImageFormat.KP_IMAGE_FORMAT_RGB565,
+                    resize_mode=kp.ResizeMode.KP_RESIZE_ENABLE,
+                    padding_mode=kp.PaddingMode.KP_PADDING_CORNER,
+                    normalize_mode=kp.NormalizeMode.KP_NORMALIZE_KNERON
+                )
+            ]
+        )
+
+       
+
+    def Inference(self):
+        """
+        starting inference work
+        """
+        print('[Starting Inference Work]')
+        print(' - ', end='')
+
+        try:
+            kp.inference.generic_image_inference_send(device_group=self.device_group,
+                                                    generic_inference_input_descriptor=self.generic_inference_input_descriptor)
+
+            self.generic_raw_result = kp.inference.generic_image_inference_receive(device_group=self.device_group)
+        except kp.ApiKPException as exception:
+            print(' - Error: inference failed, error = {}'.format(exception))
+            exit(0)
+
+            print('.', end='', flush=True)
+        print()
+
+        """
+        retrieve inference node output 
+        """
+        print('[Retrieve Inference Node Output ]')
+        self.inf_node_output_list = []
+        for node_idx in range(self.generic_raw_result.header.num_output_node):
+            inference_float_node_output = kp.inference.generic_inference_retrieve_float_node(
+                node_idx=node_idx,
+                generic_raw_result=self.generic_raw_result,
+                channels_ordering=kp.ChannelOrdering.KP_CHANNEL_ORDERING_CHW
+            )
+            self.inf_node_output_list.append(inference_float_node_output)
+
+        print(' - Success')
+
+        self.yolo_result = post_process_yolo_v5(inference_float_node_output_list=self.inf_node_output_list,
+                                        hardware_preproc_info=self.generic_raw_result.header.hw_pre_proc_info_list[0],
+                                        thresh_value=0.2)
+        
+        msg = String()
+        coords = []
+        print(" - Bounding boxes info (xmin,ymin,xmax,ymax):")
+        # output_img_name = 'output_{}'.format(os.path.basename(self.image_file_path))
+
+        for box in self.yolo_result.box_list:
+            # b = 100 + (25 * box.class_num) % 156
+            # g = 100 + (80 + 40 * box.class_num) % 156
+            # r = 100 + (120 + 60 * box.class_num) % 156
+            # color = (b, g, r)
+
+            # cv2.rectangle(img=self.img,
+            #             pt1=(int(box.x1), int(box.y1)),
+            #             pt2=(int(box.x2), int(box.y2)),
+            #             color=color,
+            #             thickness=2)
+            coords.append(f"[{int(box.x1)}, {int(box.y1)}, {int(box.x2)}, {int(box.y2)}]\n")
+       
+        msg.data = "".join(coords)
+        self.pub.publish(msg)
+        self.get_logger().info(f"Published: \n{msg.data}")
+        # cv2.imwrite(os.path.join(pkg_share, './{}'.format(output_img_name)), img=self.img)
+
+def main(args = None):
+    rclpy.init(args = args)
+    node = ImageClassifierNode_kl520_pub("image_classifier_kl520_pub")
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("shutting down...")
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

camera_kl520/object_detection_kl520_sub.py

@@ -0,0 +1,24 @@
+#! /usr/bin/env python3
+import rclpy
+from std_msgs.msg import String
+from rclpy.node import Node
+
+class ImageClassifierNode_sub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        self.sub = self.create_subscription(String, 'kl520_objection_detection/result',self.listener_callback, 10)
+       
+    def listener_callback(self, msg):
+        msg = msg.data
+        self.get_logger().info(f"I received msg \n{msg}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ImageClassifierNode_sub("image_classifier_kl520_sub")
+    if rclpy.ok:
+        rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

camera_kl520/object_detection_pub.py

@@ -0,0 +1,105 @@
+#! /usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from vision_msgs.msg import Detection2D, ObjectHypothesisWithPose, BoundingBox2D
+import cv2
+import torch
+import torchvision.transforms as transforms
+from torchvision.models import ResNet18_Weights, resnet18
+
+class ImageClassifierNode_pub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        # Create a ROS 2 Publisher to publish Detection2D messages
+        self.publisher = self.create_publisher(Detection2D, 'object_detection/result', 10)
+        # Retrieve the static image path from parameters (use absolute path)
+        self.declare_parameter('image_path', '/home/jack/workspace/ros_ur_driver/src/camera_kl520/camera_kl520/image1.jpg')
+        image_path = self.get_parameter('image_path').get_parameter_value().string_value
+
+        # Load pretrained ResNet18 model and set to evaluation mode
+        weights = ResNet18_Weights.DEFAULT
+        self.model = resnet18(weights=weights)
+        self.model.eval()  # Evaluation mode disables training-specific layers
+        # Get ImageNet class names
+        self.class_names = weights.meta["categories"]
+
+        # Read local image (for display purposes)
+        image = cv2.imread(image_path)
+        if image is None:
+            self.get_logger().error(f"Failed to read image: {image_path}")
+            return
+
+        # Preprocess image to match model input requirements
+        image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+        image_rgb = cv2.resize(image_rgb, (224, 224))
+        transform = transforms.Compose([
+            transforms.ToTensor(),  # Convert to tensor and scale values to [0,1]
+            transforms.Normalize(mean=[0.485, 0.456, 0.406],
+                                 std=[0.229, 0.224, 0.225])
+        ])
+        self.input_tensor = transform(image_rgb).unsqueeze(0)  # Add batch dimension
+        self.timer = self.create_timer(1, self.timer_callback)
+
+    def timer_callback(self):
+        # Run model inference
+        with torch.no_grad():
+            output = self.model(self.input_tensor)  # Output logits
+            probabilities = torch.nn.functional.softmax(output, dim=1)
+        score, predicted_idx = torch.max(probabilities, 1)
+        score = float(score.cpu().item())
+        predicted_idx = int(predicted_idx.cpu().item())
+
+        # Get predicted class name (fallback to index if out of range)
+        if 0 <= predicted_idx < len(self.class_names):
+            class_name = self.class_names[predicted_idx]
+        else:
+            class_name = str(predicted_idx)
+
+        # Create Detection2D message
+        detection = Detection2D()
+        detection.header.stamp = self.get_clock().now().to_msg()
+        detection.header.frame_id = 'camera_frame'
+
+        # Fill in object classification result
+        hypothesis = ObjectHypothesisWithPose()
+        hypothesis.hypothesis.class_id = class_name
+        hypothesis.hypothesis.score = score
+        detection.results.append(hypothesis)
+
+        # Publish the Detection2D message
+        self.publisher.publish(detection)
+        self.get_logger().info(f"Result class: {class_name}, Confidence: {score:.2f}")
+
+        # Example bounding box covering the entire image (commented out)
+        # bbox = BoundingBox2D()
+        # height, width, _ = image.shape
+        # # Set center x, y, theta of bbox (Pose2D fields)
+        # bbox.center.position.x = width / 2.0
+        # bbox.center.position.y = height / 2.0
+        # bbox.center.theta = 0.0  # No rotation
+        # bbox.size_x = float(width)
+        # bbox.size_y = float(height)
+        # detection.bbox = bbox
+
+        # Draw bounding box and label on image and display (commented out)
+        # top_left = (int(bbox.center.position.x - bbox.size_x / 2), int(bbox.center.position.y - bbox.size_y / 2))
+        # bottom_right = (int(bbox.center.position.x + bbox.size_x / 2), int(bbox.center.position.y + bbox.size_y / 2))
+        # cv2.rectangle(image, top_left, bottom_right, (0, 255, 0), 2)
+        # label = f"{class_name}: {score:.2f}"
+        # cv2.putText(image, label, (top_left[0], top_left[1] - 10), cv2.FONT_HERSHEY_SIMPLEX,
+        #             0.9, (0, 255, 0), 2)
+        # cv2.imshow("Detection Result", image)
+        # cv2.waitKey(0)
+        # cv2.destroyAllWindows()
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ImageClassifierNode_pub("image_classifier_pub")
+    if rclpy.ok():
+        rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

camera_kl520/object_detection_sub.py

@@ -0,0 +1,24 @@
+import rclpy
+from vision_msgs.msg import Detection2D, ObjectHypothesisWithPose
+from rclpy.node import Node
+
+class ImageClassifierNode_sub(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        self.sub = self.create_subscription(Detection2D, 'object_detection/result',self.listener_callback, 10)
+       
+    def listener_callback(self, detection):
+        hypothesis = detection.results[0].hypothesis
+        self.get_logger().info(f"Result class: {hypothesis.class_id}, Confidence: {hypothesis.score:.2f}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ImageClassifierNode_sub("image_classifier_sub")
+    if rclpy.ok:
+        rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+

camera_kl520/testing.py

@@ -0,0 +1,30 @@
+import cv2
+
+# Open webcam device 0 using the V4L2 backend
+cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
+
+if not cap.isOpened():
+    print("Failed to open camera.")
+    exit(-1)
+
+# Force MJPG pixel format for higher throughput
+fourcc = cv2.VideoWriter_fourcc(*'MJPG')
+cap.set(cv2.CAP_PROP_FOURCC, fourcc)
+
+# Configure resolution and FPS – adjust as needed
+cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) # can change to 640 or other resolution
+cap.set(cv2.CAP_PROP_FPS, 30)
+
+while True:
+    ret, frame = cap.read()
+    if not ret:
+        print("Failed to grab frame.")
+        break
+
+    # cv2.imshow('Camera', frame)
+    # if cv2.waitKey(1) & 0xFF == ord('q'):
+    #   break
+
+cap.release()
+cv2.destroyAllWindows()

camera_kl520/testing_camera.py

@@ -0,0 +1,58 @@
+#! /usr/bin/env python3
+import cv2
+import torch
+
+# Load the YOLOv5‑L model from the Ultralytics hub and move it to GPU
+model = torch.hub.load("ultralytics/yolov5", "yolov5l", pretrained=True)
+model.to("cuda")
+model.eval()
+
+
+def main():
+    """Open the default webcam, perform inference, and display annotated frames."""
+
+    # Open webcam device 0 using the V4L2 backend
+    cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
+
+    if not cap.isOpened():
+        print("Failed to open camera.")
+        exit(-1)
+
+    # Force MJPG pixel format for higher throughput
+    fourcc = cv2.VideoWriter_fourcc(*"MJPG")
+    cap.set(cv2.CAP_PROP_FOURCC, fourcc)
+
+    # Configure resolution and FPS – adjust as needed
+    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+    cap.set(cv2.CAP_PROP_FPS, 30)
+
+    while True:
+        ret, frame = cap.read()
+        if not ret:
+            print("Failed to grab frame.")
+            break
+
+        # Convert BGR (OpenCV default) to RGB for the model
+        img_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+
+        # Run inference (returns a Results object)
+        results = model(img_rgb)
+
+        # Render detections on the frame
+        annotated_frame = results.render()[0]
+        annotated_frame = cv2.cvtColor(annotated_frame, cv2.COLOR_RGB2BGR)
+
+        # Display the annotated frame
+        cv2.imshow("YOLOv5 Inference", annotated_frame)
+
+        # Press 'q' to quit
+        if cv2.waitKey(1) & 0xFF == ord("q"):
+            break
+
+    cap.release()
+    cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    main()

package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>camera_kl520</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jack23162329@gmail.com">jack</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <depend>rclpy</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

res/firmware/KL520/VERSION

@@ -0,0 +1 @@
+2.2.0

res/firmware/KL630/VERSION

@@ -0,0 +1 @@
+SDK-v2.5.7

res/firmware/KL720/VERSION

@@ -0,0 +1 @@
+2.2.0

res/firmware/KL730/VERSION

@@ -0,0 +1 @@
+SDK-v1.2.1