Meriem Ghrissi
Meriem Ghrissi

Reputation: 1

ArUco marker detection not working with ROS2 and PX4 SITL

I'm working on a project where I need to detect ArUco markers using ROS2 and PX4 SITL. I've implemented a Python script that subscribes to the camera topic and uses OpenCV to detect ArUco markers in the image stream. However, despite running the script, the markers are not being detected, and there are no errors displayed. The image window opens, but no markers are drawn

Here's a breakdown of my setup and code:

ROS2 version: [Specify your ROS2 version]
PX4 SITL version: [Specify your PX4 SITL version]
Python version: [Specify your Python version]

I'm using the following code to detect ArUco markers:

class ArucoNode(Node):
    def __init__(self):
        super().__init__('aruco_node')

        self.declare_parameter("aruco_dictionary_name", "DICT_ARUCO_ORIGINAL")
        self.declare_parameter("aruco_marker_side_length", 0.05)
        self.declare_parameter("camera_calibration_parameters_filename", "./PX4-ROS2-Gazebo-YOLOv8/calibration_chessboard.yaml")
        self.declare_parameter("image_topic", "/camera")
        self.declare_parameter("aruco_marker_name", "aruco_marker")

        aruco_dictionary_name = self.get_parameter("aruco_dictionary_name").get_parameter_value().string_value
        self.aruco_marker_side_length = self.get_parameter("aruco_marker_side_length").get_parameter_value().double_value
        self.camera_calibration_parameters_filename = self.get_parameter(
            "camera_calibration_parameters_filename").get_parameter_value().string_value
        image_topic = self.get_parameter("image_topic").get_parameter_value().string_value
        self.aruco_marker_name = self.get_parameter("aruco_marker_name").get_parameter_value().string_value

        if ARUCO_DICT.get(aruco_dictionary_name, None) is None:
            self.get_logger().info("[INFO] ArUCo tag of '{}' is not supported".format(aruco_dictionary_name))

        cv_file = cv2.FileStorage(self.camera_calibration_parameters_filename, cv2.FILE_STORAGE_READ) 
        self.mtx = cv_file.getNode('camera_matrix').mat()
        self.dst = cv_file.getNode('distortion_coefficients').mat()
        cv_file.release()

        self.get_logger().info("[INFO] detecting '{}' markers...".format(aruco_dictionary_name))
        self.this_aruco_dictionary = cv2.aruco.Dictionary_get(ARUCO_DICT[aruco_dictionary_name])
        self.this_aruco_parameters = cv2.aruco.DetectorParameters_create()

        self.subscription = self.create_subscription(
            Image, 
            image_topic, 
            self.listener_callback, 
            qos_profile=qos_profile_sensor_data)
        self.subscription # prevent unused variable warning

        self.publisher_aruco_marker_detected = self.create_publisher(Bool, 'aruco_marker_detected', 10)
        self.publisher_offset_aruco_marker = self.create_publisher(Int32, 'aruco_marker_offset', 10)
        self.offset_aruco_marker = 0

        self.bridge = CvBridge()
    def listener_callback(self, data):
        self.get_logger().info("[INFO] Image received.")
        current_frame = self.bridge.imgmsg_to_cv2(data)

        (corners, marker_ids, rejected) = cv2.aruco.detectMarkers(
            current_frame, self.this_aruco_dictionary, parameters=self.this_aruco_parameters,
            cameraMatrix=self.mtx, distCoeff=self.dst)

        aruco_detected_flag = Bool()
        aruco_detected_flag.data = False

        aruco_center_offset_msg = Int32()
        aruco_center_offset_msg.data = self.offset_aruco_marker

        image_width = current_frame.shape[1]

        if marker_ids is not None:
            aruco_detected_flag.data = True

            cv2.aruco.drawDetectedMarkers(current_frame, corners, marker_ids)

            M = cv2.moments(corners[0][0])
            cX = int(M["m10"] / M["m00"])
            cY = int(M["m01"] / M["m00"])

            self.offset_aruco_marker = cX - int(image_width/2)
            aruco_center_offset_msg.data = self.offset_aruco_marker

            cv2.putText(current_frame, "Center Offset: " + str(self.offset_aruco_marker), (cX - 40, cY - 40), 
                        cv2.FONT_HERSHEY_COMPLEX, 0.7, (0, 255, 0), 2) 

        self.publisher_aruco_marker_detected.publish(aruco_detected_flag)
        self.publisher_offset_aruco_marker.publish(aruco_center_offset_msg)


           

The script runs without errors, but no markers are detected in the image window. I would appreciate any insights or suggestions on how to troubleshoot and resolve this issue.

Thank you in advance for your help!

Upvotes: 0

Views: 143

Answers (0)

Related Questions