Reputation: 1
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