How can I send a image file or movie file to Isaac-ROS-Apriltag and detect the apriltag markers in the file?

uma7 · January 21, 2024, 10:14am

I am trying to send a .png file or .avi file to a Node, apriltag, with Isaac-ROS-Apriltag without using Isaac SIM.
However, I cannot solve it.
I think if I can control the messages like the rqt_graph image I attached, I can send the .png image.
But it is difficult and errors occurred.

Could I ask two questions?

Is it possible to send not streams from usb camera but image or movie files to Isaac-ROS-Apriltag and detect apriltag markers in the files?
If possible, what should I change in my code?

Thank you.

my code, 3d_april.py

import os, sys
import rclpy
from rclpy.node import Node
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image, CameraInfo

import logging
logging.basicConfig(
    stream=sys.stdout,
    level=logging.DEBUG,
    format="[%(asctime)s] [%(levelname)s] [%(filename)s] [Line : %(lineno)d] [module : %(module)s] [PID : %(process)d] %(message)s")


class Send_3d_April(Node):
    
    def __init__(self):
        super().__init__("send_3d_april") 
        self.publisher_image = self.create_publisher(Image, "image", 10) 
        self.publisher_camera_info = self.create_publisher(CameraInfo, "camera_info", 10)
        timer_period = 1
        self.timer = self.create_timer(timer_period, self.send_3d_image_producer_callback)
        self.i = 0


    def send_3d_image_producer_callback(self):
        # I referred to `$ play quickstart.bag`and set these parameters 
        msg_image = Image()
        msg_image.header.stamp.sec = 333            # uint8
        msg_image.header.stamp.nanosec = 716684071  # uint32
        msg_image.header.frame_id = "3d_image"      # string
        msg_image.width = 1920                      # uint32
        msg_image.height = 1080                     # uint32
        msg_image.encoding = "rgb8"                 # string
        msg_image.is_bigendian = 0                  # uint8
        msg_image.step = 5760                       # uint32

        cv_bridge = CvBridge()
        cv_image = cv2.imread("tag36h11_00555.png", cv2.IMREAD_COLOR)
        cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) 
        try:
            # TODO!
            # An exception occurred.". / The 'data' field must be a set or sequence and each value of type 'int' and each unsigned integer in [0, 255]`
            # msg_image.data = cv_bridge.cv2_to_imgmsg(cv_image, encoding="rgb8") 
            # logging.debug(f"Type of msg_image / {type(self.bridge.cv2_to_imgmsg(np.array(self.cv_image), encoding='bgr8'))}")  # <class 'sensor_msgs.msg._image.Image'>

            # So, I use following this instead.
            msg_image.data = np.array(cv_image).flatten().tolist()
        except CvBridgeError as err:
            logging.error(f"Error occured when using CvBridge: {err}")

        msg_camera_info = CameraInfo()
        msg_camera_info.header.stamp.sec = 333
        msg_camera_info.header.stamp.nanosec = 300017382
        msg_camera_info.header.frame_id = "3d_image"
        msg_camera_info.width = 1920
        msg_camera_info.width = 1080
        msg_camera_info.distortion_model = "pinhole"
        msg_camera_info.d = []
        msg_camera_info.k = [
            1465.99853515625,
            0.0,
            640.0,
            0.0,
            1468.26416015625,
            360.0,
            0.0,
            0.0,
            1.0,
        ]
        msg_camera_info.r = [
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
        ]
        msg_camera_info.p = [
            1465.99853515625,
            0.0,
            640.0,
            0.0,
            0.0,
            1468.26416015625,
            360.0,
            0.0,
            0.0,
            0.0,
            1.0,
            0.0,
        ]
        msg_camera_info.binning_x = 0 
        msg_camera_info.binning_y = 0 
        msg_camera_info.roi.x_offset = 0
        msg_camera_info.roi.y_offset = 0
        msg_camera_info.roi.height = 0
        msg_camera_info.roi.width = 0
        msg_camera_info.roi.do_rectify = False

        self.publisher.publish(msg_image)     
        self.publisher.publish(msg_camera_info) 

        # self.get_logger().info(f"Publishing, {msg_image.data}")

        self.i += 1
        # self.destroy_timer(self.timer)


def main(args=None):
    try:
        logging.info("Send 3D image to Apriltag Node.")
        rclpy.init(args=args)
        send_3d_april = Send_3d_April()
        rclpy.spin_once(send_3d_april)
    except KeyboardInterrupt as err:
        logging.error('Interrupted by Keybord "ctrl + C". / {}'.format(err))
    except Exception as exc:
        logging.error('An exception occurred.". / {}'.format(exc))
    finally:
        send_3d_april.destroy_node()
        rclpy.shutdown()
        logging.info("Node, send_3D_image_to_apriltag, is terminated.")


if __name__ == '__main__':
    main()

No info is shown after run 3d_april.py as $ python 3d_april.py.

uma7@uma7:~$ cd ${ISAAC_ROS_WS}/src/isaac_ros_common && ./scripts/run_dev.sh
admin@uma7:/workspaces/isaac_ros-dev$ ros2 topic echo /tag_detections

And if I run 3d_april.py twice, this error occurred…

admin@uma7:/workspaces/isaac_ros-dev$ ros2 launch isaac_ros_apriltag isaac_ros_apriltag.launch.py

[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:97 code=2(cudaErrorMemoryAllocation) "cudaMalloc((void**)&d_gradient_x2, sizeof(float) * gradient_w * gradient_h)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:98 code=2(cudaErrorMemoryAllocation) "cudaMalloc((void**)&d_gradient_y2, sizeof(float) * gradient_w * gradient_h)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:99 code=2(cudaErrorMemoryAllocation) "cudaMalloc((void**)&d_gradient_xy, sizeof(float) * gradient_w * gradient_h)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:100 code=2(cudaErrorMemoryAllocation) "cudaMalloc((void**)&d_blur_gradient_x2, sizeof(float) * blur_gradient_w * blur_gradient_h)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:101 code=2(cudaErrorMemoryAllocation) "cudaMalloc((void**)&d_blur_gradient_y2, sizeof(float) * blur_gradient_w * blur_gradient_h)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:102 code=2(cudaErrorMemoryAllocation) "cudaMalloc((void**)&d_blur_gradient_xy, sizeof(float) * blur_gradient_w * blur_gradient_h)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:104 code=2(cudaErrorMemoryAllocation) "cudaMalloc((void**)&d_maxima_suppression, sizeof(bool) * corners_w * corners_h)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:186 code=700(cudaErrorIllegalAddress) "cudaEventRecord(sync_event_x2, main_stream)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:187 code=700(cudaErrorIllegalAddress) "cudaStreamWaitEvent(x2_stream, sync_event_x2, 0)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:196 code=700(cudaErrorIllegalAddress) "cudaEventRecord(sync_event_y2, main_stream)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:197 code=700(cudaErrorIllegalAddress) "cudaStreamWaitEvent(y2_stream, sync_event_y2, 0)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:217 code=700(cudaErrorIllegalAddress) "cudaEventRecord(sync_event_x2, x2_stream)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:218 code=700(cudaErrorIllegalAddress) "cudaStreamWaitEvent(main_stream, sync_event_x2, 0)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:219 code=700(cudaErrorIllegalAddress) "cudaEventRecord(sync_event_y2, y2_stream)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:220 code=700(cudaErrorIllegalAddress) "cudaStreamWaitEvent(main_stream, sync_event_y2, 0)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/corner_detection/corner_detector.cpp:151 code=700(cudaErrorIllegalAddress) "cudaMemsetAsync(d_maxima_suppression, 1, sizeof(bool) * corners_w * corners_h, main_stream)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/quad_detection/quad_detector.cpp:32 code=700(cudaErrorIllegalAddress) "cudaMemsetAsync(bound_corners_cnt_.Data(), 0, sizeof(uint32_t), boundary_stream_)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/quad_detection/quad_detector.cpp:34 code=700(cudaErrorIllegalAddress) "cudaEventRecord(sync_event_, corner_stream_)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/quad_detection/quad_detector.cpp:35 code=700(cudaErrorIllegalAddress) "cudaStreamWaitEvent(boundary_stream_, sync_event_, 0)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/quad_detection/quad_detector.cpp:162 code=700(cudaErrorIllegalAddress) "cudaMemcpy(&corner_cnt, bound_corners_cnt_.Data(), sizeof(uint32_t), cudaMemcpyDefault)" 
[component_container_mt-1] 2024-01-21 22:38:13.526 ERROR gxf/std/unbounded_allocator.cpp@99: Failure in cudaFree. cuda_error: cudaErrorIllegalAddress, error_str: an illegal memory access was encountered
[ERROR] [component_container_mt-1]: process has died [pid 4072, exit code -11, cmd '/opt/ros/humble/lib/rclcpp_components/component_container_mt --ros-args -r __node:=apriltag_container -r __ns:=/'].

And if I set data to Image/data as coding msg_image.data = cv_bridge.cv2_to_imgmsg(np.array(cv_image), encoding="rgb8"), I encounter this error.

uma7@uma7:~$ cd ${ISAAC_ROS_WS}/src/isaac_ros_common && ./scripts/run_dev.sh
admin@uma7:/workspaces/isaac_ros-dev$ python 3d_april.py
An exception occurred.". / The 'data' field must be a set or sequence and each value of type 'int' and each unsigned integer in [0, 255]`

Raffaello · January 21, 2024, 7:41pm

Hi @uma7

Thank you for writing a post on this forum. I quickly reply to your post:

Is it possible to send not streams from usb camera but image or movie files to Isaac-ROS-Apriltag and detect apriltag markers in the files?

Yes, sure you just need to connect a topic sensor_msgs/Image and sensor_msgs/CameraInfo follow the API documentation on Isaac ROS apriltag

If possible, what should I change in my code?

I tried setting up and testing your code, but I encountered several issues, such as multiple exceptions and errors in the Camera Info Setup. Therefore, I took the liberty of writing a new script for you, and it seems to be working properly.

Please let me know if the problem has been resolved.

import rclpy
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, CameraInfo
import cv2
import numpy as np


class Send_3d_April(Node):

    def __init__(self):
        super().__init__("send_3d_april")
        self.publisher_image = self.create_publisher(Image, "image", 10)
        self.publisher_camera_info = self.create_publisher(CameraInfo, "camera_info", 10)
        timer_period = 1
        self.timer = self.create_timer(timer_period, self.send_3d_image_producer_callback)
        self.i = 0
        self.cv_image = cv2.imread('test.jpg')  # an RGB image

        self.bridge = CvBridge()
        self._camera_info = CameraInfo()
        self._camera_info.header.frame_id = '3d_image'
        self._camera_info.height = 1920
        self._camera_info.width = 1080
        self._camera_info.distortion_model = 'pinhole'
        self._camera_info.d = []

        self._camera_info.k = [
            1465.99853515625,
            0.0,
            640.0,
            0.0,
            1468.26416015625,
            360.0,
            0.0,
            0.0,
            1.0,]
        self._camera_info.r = [
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,]

        self._camera_info.p = [
            1465.99853515625,
            0.0,
            640.0,
            0.0,
            0.0,
            1468.26416015625,
            360.0,
            0.0,
            0.0,
            0.0,
            1.0,
            0.0,]
        self._camera_info.binning_x = 0
        self._camera_info.binning_y = 0
        self._camera_info.roi.do_rectify = False

    def send_3d_image_producer_callback(self):
        image = self.bridge.cv2_to_imgmsg(np.array(self.cv_image), "bgr8")
        self.publisher_image.publish(image)
        self.publisher_camera_info.publish(self._camera_info)
        self.get_logger().info('Publishing an image')


def main(args=None):
    rclpy.init(args=args)
    send_3d_april = Send_3d_April()
    rclpy.spin(send_3d_april)
    send_3d_april.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

uma7 · January 22, 2024, 5:28am

Hi, @Raffaello.
Thank you for your reply and advice.

I tried your code as below in the environment of Isaac-ROS-Apriltag.
I got an error on Terminal3 with spin(send_3d_april).
And even if I use spin_once(send_3d_april), it seemed apriltag system cannot detect any judging from Termial2.
In addition, my subscriber, my_tag_detections.py, also outputted any detected data.

What am I supposed to do for this?

Thank you.

■Terminal1

uma7@uma7:~$ cd ${ISAAC_ROS_WS}/src/isaac_ros_common && ./scripts/run_dev.sh
admin@uma7:/workspaces/isaac_ros-dev$ ls
build    log    src    install    tag36h11_00555.png    3d_april.py    my_tag_detections.py 
admin@uma7:/workspaces/isaac_ros-dev$ python 3d_april.py # with spin()
[INFO] [1705899367.800193068] [send_3d_april]: Publishing an image
[INFO] [1705899368.781460263] [send_3d_april]: Publishing an image
...

■Terminal2

admin@uma7:/workspaces/isaac_ros-dev$ ros2 topic echo \tag_detections # with spin()
header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
detections: []
---
header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
detections: []
---

■Terminal3

admin@uma7:/workspaces/isaac_ros-dev$ ros2 launch isaac_ros_apriltag isaac_ros_apriltag.launch.py # with spin()

[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/decoding/TagDecoder.cpp:424 code=12(cudaErrorInvalidPitchValue) "cudaMemcpy2D(h_img_data_, img_width_ * sizeof(uchar3), img_gpu.data_, img_gpu.pitch_, img_width_ * sizeof(uchar3), img_height_, cudaMemcpyDefault)" 
[component_container_mt-1] CUDA error at /workspaces/isaac_ros-dev/lib/src/cuapriltags/april_tagging/decoding/TagDecoder.cpp:424 code=12(cudaErrorInvalidPitchValue) "cudaMemcpy2D(h_img_data_, img_width_ * sizeof(uchar3), img_gpu.data_, img_gpu.pitch_, img_width_ * sizeof(uchar3), img_height_, cudaMemcpyDefault)" 
...

my_tag_detections.py

import os, sys
import json
import rclpy
from std_msgs.msg import String
from isaac_ros_apriltag_interfaces.msg import AprilTagDetectionArray

from rclpy.node import Node

import logging
logging.basicConfig(
    stream=sys.stdout,
    level=logging.DEBUG,
    format="[%(asctime)s] [%(levelname)s] [%(filename)s] [Line : %(lineno)d] [module : %(module)s] [PID : %(process)d] %(message)s")


class MyTagDetections(Node):
    def __init__(self):
        super().__init__("my_tag_detections")
        self.sub = self.create_subscription(AprilTagDetectionArray, "tag_detections", self.tag_detections_callback, 10) 

    def tag_detections_callback(self, msg): 
        if not msg.detections:
            logging.info("Data is empty.")
        else:
            data = {}
            # header
            data["header"] = {}
            data["header"]["timestamp_sec"] = msg.header.stamp.sec
            data["header"]["timestamp_nanosec"] = msg.header.stamp.nanosec
            data["header"]["frame_id"] = msg.header.frame_id
            data["detections"] = {}
            data["detections"]["family"] = msg.detections[0].family
            data["detections"]["id"] = msg.detections[0].id
            data["detections"]["position"] = {}
            data["detections"]["position"]["x"] = msg.detections[0].pose.pose.pose.position.x
            data["detections"]["position"]["y"] = msg.detections[0].pose.pose.pose.position.y
            data["detections"]["position"]["z"] = msg.detections[0].pose.pose.pose.position.z
            data["detections"]["orientation"] = {}
            data["detections"]["orientation"]["x"] = msg.detections[0].pose.pose.pose.orientation.x
            data["detections"]["orientation"]["y"] = msg.detections[0].pose.pose.pose.orientation.y
            data["detections"]["orientation"]["z"] = msg.detections[0].pose.pose.pose.orientation.z
            data["detections"]["orientation"]["w"] = msg.detections[0].pose.pose.pose.orientation.w

            self.get_logger().info(f"Subscribe: {msg.data}")
            

def main(args=None):
    try:
        logging.info('Trying to catch detected data from apriltag.')
        rclpy.init(args=args)
        node = MyTagDetections()
        rclpy.spin(node)
    except KeyboardInterrupt as err:
        logging.warning('Interrupted by Keybord "ctrl + C". / {}'.format(err))
    except Exception as exc:
        logging.warning('An exception occurred.". / {}'.format(exc))
    finally:
        node.destroy_node()
        rclpy.shutdown()
        logging.info('Node, my_tag_detections is terminated.')


if __name__ == '__main__':
    main()

■Terminal4

admin@uma7:/workspaces/isaac_ros-dev$ python my_tag_detections.py # with spin_once()
[2024-01-22 17:48:21,611] [INFO] [my_tag_detections.py] [Line : 76] [module : my_tag_detections] [PID : 1859] Trying to catch detected data from apriltag.
...

admin@uma7:/workspaces/isaac_ros-dev$ python my_tag_detections.py # with spin()
[2024-01-22 17:48:21,611] [INFO] [my_tag_detections.py] [Line : 76] [module : my_tag_detections] [PID : 1859] Trying to catch detected data from apriltag.
[2024-01-22 18:06:49,826] [INFO] [my_tag_detections.py] [Line : 47] [module : my_tag_detections] [PID : 1859] Data is empty.
...

■IInfo

admin@uma7:/workspaces/isaac_ros-dev$ nvidia-smi
Mon Jan 22 16:09:25 2024       
+---------------------------------------------------------------------------------------+
| NVIDIA-SMI 535.104.05             Driver Version: 535.104.05   CUDA Version: 12.2     |
|-----------------------------------------+----------------------+----------------------+
| GPU  Name                 Persistence-M | Bus-Id        Disp.A | Volatile Uncorr. ECC |
| Fan  Temp   Perf          Pwr:Usage/Cap |         Memory-Usage | GPU-Util  Compute M. |
|                                         |                      |               MIG M. |
|=========================================+======================+======================|
|   0  NVIDIA GeForce RTX 3060 ...    On  | 00000000:01:00.0  On |                  N/A |
| N/A   52C    P8              15W /  80W |    138MiB /  6144MiB |     54%      Default |
|                                         |                      |                  N/A |
+-----------------------------------------+----------------------+----------------------+
                                                                                         
+---------------------------------------------------------------------------------------+
| Processes:                                                                            |
|  GPU   GI   CI        PID   Type   Process name                            GPU Memory |
|        ID   ID                                                             Usage      |
|=======================================================================================|
+---------------------------------------------------------------------------------------+

admin@uma7:/workspaces/isaac_ros-dev$ nvcc -V
nvcc: NVIDIA (R) Cuda compiler driver
Copyright (c) 2005-2022 NVIDIA Corporation
Built on Wed_Sep_21_10:33:58_PDT_2022
Cuda compilation tools, release 11.8, V11.8.89
Build cuda_11.8.r11.8/compiler.31833905_0

uma7 · January 22, 2024, 12:05pm

All of sudden, it became to run without errors.
Maybe because the values of self._camera_info.height and self._camera_info.width were not proper.

When I used another image data, there were errors…
So, would you mind letting me ask you again if I can not solve the problems?

Anyway, I could see the code run properly.
@Raffaello , Thank you so much.

Raffaello · January 23, 2024, 5:32pm

Hi @uma7

I set up your self._camera_info.height and self._camera_info.width coping and pasting the values on your code. (note: on your code, you used twice the parameter width on msg_camera_info)
Please check your image and fill in the parameters from my example code.

Looking at your terminal appears multiple time the CUDA error. Please check on your host machine to if is fully installed all requirements for x86_64

system · February 6, 2024, 5:33pm

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Argus_camera not working with my new customer camera device Jetson TX2	28	4611	October 18, 2021
CUDA error 801: cudaErrorNotSupported - operation not supported Isaac Sim camera , cuda , isaacsim , isaac-sim-v4-0-0	2	52	October 7, 2024
R35.3.1 AGX ORIN bring up raw14 format，Kernel error Jetson AGX Orin camera	54	1664	October 19, 2023
Device Memeroy allocation and data transfer Data transfer between host and device CUDA Programming and Performance	5	2553	June 16, 2011
Enabling AR0330 cameras on Xavier-Nx Jetson Xavier NX camera	6	1790	October 18, 2021
Timeout on /dev/video3 capture Jetson AGX Orin camera	9	473	February 1, 2024
Jetson AGX Xavier receive MIPI CSI-2 without I2C from FPGA Jetson AGX Xavier camera	12	1279	June 25, 2021
Virtual Channel Aggreggation in VI-Bypass Mode Jetson TX2 camera	6	139	July 2, 2024
Jetson Xavier yuv422 camera driver [one camera] no reply from camera processor Jetson AGX Xavier camera	35	3510	October 17, 2021
Nvgstcapture can't display video，no cameras Jetson Orin Nano camera	25	624	September 25, 2023

How can I send a image file or movie file to Isaac-ROS-Apriltag and detect the apriltag markers in the file?

Related topics