diff --git a/src/autonomous_navigation/autonomous_navigation/object_detection.py b/src/autonomous_navigation/autonomous_navigation/object_detection.py
index 348ee076..3872c5c2 100644
--- a/src/autonomous_navigation/autonomous_navigation/object_detection.py
+++ b/src/autonomous_navigation/autonomous_navigation/object_detection.py
@@ -57,6 +57,16 @@ def __init__(self):
cv2.aruco.DICT_4X4_50
)
self.aruco_params = cv2.aruco.DetectorParameters()
+ self.aruco_detector = cv2.aruco.ArucoDetector(
+ self.aruco_dict, self.aruco_params)
+
+ half = self.marker_length / 2.0
+ self.marker_obj_points = np.array([
+ [-half, half, 0],
+ [ half, half, 0],
+ [ half, -half, 0],
+ [-half, -half, 0],
+ ], dtype=np.float32)
self.get_logger().info('ArUco object detection node started.')
@@ -93,10 +103,7 @@ def image_cb(self, img_msg: Image):
cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGRA2BGR)
gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
- # Detect markers
- corners, ids, _ = cv2.aruco.detectMarkers(
- gray, self.aruco_dict, parameters=self.aruco_params
- )
+ corners, ids, _ = self.aruco_detector.detectMarkers(gray)
if ids is None:
return
@@ -108,15 +115,14 @@ def image_cb(self, img_msg: Image):
# Estimate pose of all detected markers
# rvecs, tvecs shape=(N,1,3)
- rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
- corners,
- self.marker_length,
- self.camera_matrix,
- self.dist_coeffs
- )
+ success, rvec, tvec = cv2.solvePnP(
+ self.marker_obj_points, corners[idx][0],
+ self.camera_matrix, self.dist_coeffs)
+ if not success:
+ return
# pick our target’s translation vector
- t = tvecs[idx][0] # [x_cam, y_cam, z_cam] in OpenCV camera frame
+ t = tvec.flatten() # [x_cam, y_cam, z_cam] in OpenCV camera frame
# Convert to your robot frame:
# OpenCV camera frame: x→right, y→down, z→forward
diff --git a/src/cmr_arm_arucos/cmr_arm_arucos/aruco_detection_node.py b/src/cmr_arm_arucos/cmr_arm_arucos/aruco_detection_node.py
index 6a2fe12b..fa0dacf5 100644
--- a/src/cmr_arm_arucos/cmr_arm_arucos/aruco_detection_node.py
+++ b/src/cmr_arm_arucos/cmr_arm_arucos/aruco_detection_node.py
@@ -1,5 +1,6 @@
import rclpy
from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
@@ -9,6 +10,7 @@
from std_msgs.msg import Float32MultiArray, Int32MultiArray
from collections import Counter
import time
+import threading
class ArucoDetectionNode(Node):
def __init__(self):
@@ -16,7 +18,7 @@ def __init__(self):
self.subscription = self.create_subscription(Image, '/zed/image_left', self.image_callback, 10)
self.plane_subscription = self.create_subscription(
- Float32MultiArray, '/zed/plane/angles', self.plane_callback, 10
+ Float32MultiArray, '/zed/plane/equation', self.plane_callback, 10
)
self.bridge = CvBridge()
@@ -36,6 +38,15 @@ def __init__(self):
[0.0, 0.0, 1.0]])
self.dist_coeffs = np.array([0.34688736, 0.07662388, 0.14965771, 0.01600403])
+ self.marker_length = 0.1
+ half = self.marker_length / 2.0
+ self.marker_obj_points = np.array([
+ [-half, half, 0],
+ [ half, half, 0],
+ [ half, -half, 0],
+ [-half, -half, 0],
+ ], dtype=np.float32)
+
# Joint info log (computed from relavent aruco tag/angle)
# Key - joint index (0 is base joint, 5 is last joint)
# Value - (relevant fiducial id, value of depth reading to try and maximize zero position,
@@ -63,92 +74,63 @@ def __init__(self):
2: {'yaw': [], 'pitch': [], 'roll': []},
3: {'yaw': [], 'pitch': [], 'roll': []}}
- self.count = 0
self.rotations_cache = {0: {'yaw': [], 'pitch': [], 'roll': []},
1: {'yaw': [], 'pitch': [], 'roll': []},
2: {'yaw': [], 'pitch': [], 'roll': []},
3: {'yaw': [], 'pitch': [], 'roll': []}}
- # Publisher to send joint angle increments to ArmControllerNode
- self.joint_increment_publisher = self.create_publisher(
- Float32MultiArray, '/arm/joint_increment', 10)
-
self.get_logger().info("Arm Aruco Detection Node has started.")
def image_callback(self, msg):
-
- #try:
-
- cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
- annotated_image = self.detect_aruco_markers(cv_image, self.camera_matrix, self.dist_coeffs)
-
- if (annotated_image is not None):
- self.detected_publisher.publish(self.bridge.cv2_to_imgmsg(annotated_image, encoding='bgr8'))
+ try:
+ cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
+ annotated_image = self.detect_aruco_markers(cv_image)
- #except Exception as e:
- #self.get_logger().error(f"Failed to process image: {e}")
+ if annotated_image is not None:
+ self.detected_publisher.publish(self.bridge.cv2_to_imgmsg(annotated_image, encoding='bgr8'))
+ except Exception as e:
+ self.get_logger().error(f"Failed to process image: {e}")
- def detect_aruco_markers(self, image, camera_matrix, dist_coeffs, dictionary=cv2.aruco.DICT_6X6_50):
+ def detect_aruco_markers(self, image, dictionary=cv2.aruco.DICT_6X6_50):
if image is None or image.size == 0:
- print("Invalid image. Skipping marker detection.")
- return []
+ self.get_logger().warn("Invalid image. Skipping marker detection.")
+ return None
- if image.shape[-1] == 4: # Convert RGBA to BGR
+ if image.shape[-1] == 4:
image = cv2.cvtColor(image, cv2.COLOR_RGBA2BGR)
-
aruco_dict = cv2.aruco.getPredefinedDictionary(dictionary)
parameters = cv2.aruco.DetectorParameters()
+ detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
- corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
+ corners, ids, _ = detector.detectMarkers(gray)
- camera_matrix = self.camera_matrix
- dist_coeffs = self.dist_coeffs
- angles = {}
ids_in_frame = []
if ids is not None:
-
cv2.aruco.drawDetectedMarkers(image, corners, ids)
- rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(corners, 0.1, camera_matrix, dist_coeffs)
for i in range(len(ids)):
- if ids[i][0] not in self.aruco_rotations.keys():
+ if ids[i][0] not in self.aruco_rotations:
continue
ids_in_frame.append(ids[i][0])
- # Calculate centerpoint of marker
- marker_corners = corners[i][0] # Corners are stored as a (1, 4, 2) array
- center_x = int(np.mean(marker_corners[:, 0])) # Average of x-coordinates
- center_y = int(np.mean(marker_corners[:, 1])) # Average of y-coordinates
+ marker_corners = corners[i][0]
+ center_x = int(np.mean(marker_corners[:, 0]))
+ center_y = int(np.mean(marker_corners[:, 1]))
self.current_detections[ids[i][0]] = (center_x, center_y)
- '''rotation_matrix, _ = cv2.Rodrigues(rvecs[i][0])
- yaw, pitch, roll = self.rotation_matrix_to_euler_angles(rotation_matrix)
- angles[ids[i][0]] = {}
- angles[ids[i][0]]['yaw'] = yaw
- angles[ids[i][0]]['pitch'] = pitch
- angles[ids[i][0]]['roll'] = roll
- # Extract unit vectors for X, Y, Z axes
- x_axis = rotation_matrix[:, 0] # X-axis unit vector
- y_axis = rotation_matrix[:, 1] # Y-axis unit vector
- z_axis = rotation_matrix[:, 2] # Z-axis unit vector
- f_desired = (np.array([1, 0, 0]), np.array([0, 0, 1]), np.array([0, -1, 0]))
- f_current = (np.array(x_axis), np.array(y_axis), np.array(z_axis))
-
- yaw, _, _ = self.compute_euler_angles(f_desired, f_current)
- f_desired = self.transform_coordinate_frame(f_desired, (yaw, 0, 0))
- _, _, roll = self.compute_euler_angles(f_desired, f_current)
-
- self.get_logger().info(f"Z axis: {z_axis}")
- self.get_logger().info(f"Base rotation: {yaw} Shoulder rotation: {roll}")'''
- cv2.drawFrameAxes(image, camera_matrix, dist_coeffs, rvecs[i][0], tvecs[i][0], 0.1)
-
+
+ success, rvec, tvec = cv2.solvePnP(
+ self.marker_obj_points, corners[i][0],
+ self.camera_matrix, self.dist_coeffs)
+ if success:
+ cv2.drawFrameAxes(image, self.camera_matrix, self.dist_coeffs, rvec, tvec, 0.1)
+
if len(ids_in_frame) != 4:
self.get_logger().info(f"Not all arucos in frame. Arucos currently in frame: {ids_in_frame}")
-
elif self.arm_homing:
self.request_planes()
-
+
return image
def request_planes(self):
@@ -178,37 +160,50 @@ def request_plane(self, x, y):
self.plane_requester.publish(pixel_msg)
'''
- def plane_callback(self,msg):
+ def plane_callback(self, msg):
"""
- Store the aruco rotations requested from the ZED
+ Store the aruco rotations derived from plane equations returned by the ZED.
+ Data arrives in groups of 5: [marker_id, a, b, c, d] where ax+by+cz=d.
"""
- angles = msg.data
-
- for i in range(0, len(angles), 4):
- id = int(angles[i])
- yaw = angles[i+1]
- pitch = angles[i+2]
- roll = angles[i+3]
- self.aruco_rotations[id]['yaw'].append(yaw)
- self.aruco_rotations[id]['pitch'].append(pitch)
- self.aruco_rotations[id]['roll'].append(roll)
-
- home = True
- if self.arm_homing:
- for marker in self.aruco_rotations.keys():
+ data = msg.data
- if len(self.aruco_rotations[marker]['yaw']) < self.required_count:
- home = False
- self.get_logger().info(f"Marker {marker}, angles collected: {len(self.aruco_rotations[marker]['yaw'])}")
- break
+ for i in range(0, len(data), 5):
+ marker_id = int(data[i])
+ a, b, c = data[i + 1], data[i + 2], data[i + 3]
+ normal = np.array([a, b, c])
+ roll, pitch, yaw = self.compute_roll_pitch_yaw_from_normal(normal)
- if home:
- self.arm_homing = False
- self.execute_arm_homing()
+ if marker_id not in self.aruco_rotations:
+ continue
+ self.aruco_rotations[marker_id]['yaw'].append(yaw)
+ self.aruco_rotations[marker_id]['pitch'].append(pitch)
+ self.aruco_rotations[marker_id]['roll'].append(roll)
+
+ if not self.arm_homing:
+ return
+
+ for marker in self.aruco_rotations:
+ if len(self.aruco_rotations[marker]['yaw']) < self.required_count:
+ self.get_logger().info(f"Marker {marker}, angles collected: {len(self.aruco_rotations[marker]['yaw'])}")
+ return
+
+ self.arm_homing = False
+ threading.Thread(target=self.execute_arm_homing, daemon=True).start()
+ def compute_roll_pitch_yaw_from_normal(self, normal):
+ """
+ Compute roll, pitch, yaw from a plane normal vector, matching the
+ ZED publisher's compute_roll_pitch_yaw convention.
+ """
+ normal = normal / np.linalg.norm(normal)
+ nx, ny, nz = normal
+ pitch = math.degrees(np.arcsin(-nx))
+ yaw = math.degrees(np.arctan2(ny, nz))
+ roll = 0.0
+ return roll, pitch, yaw
+
def execute_arm_homing(self):
-
self.get_logger().info("HOMING ARM")
# Get the joint correction angles
corrections = self.filter_detections()
@@ -264,9 +259,8 @@ def execute_arm_homing(self):
self.offset_publisher.publish(elbow_msg)
time.sleep(5.0)'''
- # Clear the detection cache
- for id in self.aruco_rotations.keys():
- self.aruco_rotations[id] = {'yaw': [], 'pitch': [], 'roll': []}
+ for marker_id in self.aruco_rotations:
+ self.aruco_rotations[marker_id] = {'yaw': [], 'pitch': [], 'roll': []}
@@ -383,46 +377,6 @@ def rotation_matrix_to_euler_angles(self, R):
return math.degrees(yaw), math.degrees(pitch), math.degrees(roll)
- def move_joint_to_pos(self, joint, pos):
- """
- Moves joint to position relative to current (in degrees)
- """
-
- joint_increment_msg = Float32MultiArray()
- # Assuming joint index 0 is the base joint
- swap = self.joint_info[joint][3]
- increments = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-
- increments[joint] = math.radians(pos*swap)
-
- joint_increment_msg.data = increments
- self.joint_increment_publisher.publish(joint_increment_msg)
-
- def increment_joint(self):
- """
- Publishes a message to increment joint angles.
-
- :param joint: Which joint to increment.
- """
-
- joint = self.current_joint
- joint_increment_msg = Float32MultiArray()
- # Assuming joint index 0 is the base joint
- swap = self.joint_info[joint][3]
- increments = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
- if self.curr_offset == self.search_start:
- increments[joint] = math.radians(swap * self.search_start)
- self.curr_offset += 1.0
- elif self.curr_offset >= self.search_end:
- self.get_best_offset()
- return
- else:
- self.curr_offset += 1.0
- increments[joint] = math.radians(swap)
-
- joint_increment_msg.data = increments
- self.joint_increment_publisher.publish(joint_increment_msg)
-
def round_and_find_most_common(self, array):
"""
Rounds all elements of a numpy array to the nearest ones place
@@ -536,31 +490,6 @@ def transform_coordinate_frame(self, frame, euler_angles):
return x_new, y_new, z_new
- def rotation_matrix_to_euler_angles(self, R):
- """
- Convert a rotation matrix to Euler angles (yaw, pitch, roll).
- Assumes ZYX rotation order.
-
- Parameters:
- R (np.ndarray): 3x3 rotation matrix.
-
- Returns:
- tuple: (yaw, pitch, roll) in radians.
- """
- sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
- singular = sy < 1e-6
-
- if not singular:
- yaw = np.arctan2(R[2, 1], R[2, 2])
- pitch = np.arctan2(-R[2, 0], sy)
- roll = np.arctan2(R[1, 0], R[0, 0])
- else:
- yaw = np.arctan2(-R[1, 2], R[1, 1])
- pitch = np.arctan2(-R[2, 0], sy)
- roll = 0
-
- return yaw, pitch, roll
-
def rotate_v2_to_plane(self, v1, v2, v3):
"""
Rotate v2 about v1 until v2 lies in the plane defined by v1 and v3.
@@ -599,8 +528,10 @@ def rotate_v2_to_plane(self, v1, v2, v3):
def main(args=None):
rclpy.init(args=args)
node = ArucoDetectionNode()
+ executor = MultiThreadedExecutor()
+ executor.add_node(node)
try:
- rclpy.spin(node)
+ executor.spin()
except KeyboardInterrupt:
pass
finally:
diff --git a/src/cmr_arm_arucos/cmr_arm_arucos/simple_camera_node.py b/src/cmr_arm_arucos/cmr_arm_arucos/simple_camera_node.py
new file mode 100644
index 00000000..8df1e2eb
--- /dev/null
+++ b/src/cmr_arm_arucos/cmr_arm_arucos/simple_camera_node.py
@@ -0,0 +1,146 @@
+"""
+Lightweight camera publisher using cv2.VideoCapture.
+Works with any USB camera (including ZED as a standard UVC device)
+without requiring the ZED SDK (pyzed).
+
+Auto-detects available video devices and tries each one.
+"""
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+import cv2
+import time
+import glob
+import subprocess
+import os
+
+
+class SimpleCameraNode(Node):
+ def __init__(self):
+ super().__init__('simple_camera_node')
+
+ self.declare_parameter('camera_index', -1)
+ self.declare_parameter('fps', 10.0)
+ self.declare_parameter('image_topic', '/zed/image_left')
+ self.declare_parameter('kill_competing', True)
+
+ camera_index = self.get_parameter('camera_index').value
+ fps = float(self.get_parameter('fps').value)
+ image_topic = self.get_parameter('image_topic').value
+ kill_competing = self.get_parameter('kill_competing').value
+
+ self.publisher = self.create_publisher(Image, image_topic, 10)
+ self.bridge = CvBridge()
+
+ if kill_competing:
+ self._kill_competing_processes()
+
+ self.cap = self._open_camera(camera_index)
+
+ for _ in range(10):
+ ret, _ = self.cap.read()
+ if ret:
+ break
+ time.sleep(0.2)
+
+ self.timer = self.create_timer(1.0 / fps, self.publish_frame)
+ self.get_logger().info(
+ f'Camera opened, publishing to {image_topic} at {fps} FPS')
+
+ def _kill_competing_processes(self):
+ """Kill other processes that may be holding /dev/video* devices."""
+ try:
+ video_devices = glob.glob('/dev/video*')
+ if not video_devices:
+ return
+ result = subprocess.run(
+ ['fuser'] + video_devices,
+ capture_output=True, text=True, timeout=5)
+ pids = result.stdout.strip().split()
+ my_pid = str(os.getpid())
+ for pid in pids:
+ pid = pid.strip()
+ if pid and pid != my_pid:
+ self.get_logger().warn(f'Killing process {pid} holding camera device')
+ subprocess.run(['kill', '-9', pid], timeout=5)
+ if pids:
+ time.sleep(1.0)
+ except Exception as e:
+ self.get_logger().warn(f'Could not check/kill competing processes: {e}')
+
+ def _open_camera(self, requested_index):
+ """Try to open a camera, auto-detecting if index is -1."""
+ video_devices = sorted(glob.glob('/dev/video*'))
+ self.get_logger().info(f'Available video devices: {video_devices}')
+
+ if requested_index >= 0:
+ indices = [requested_index]
+ else:
+ indices = []
+ for dev in video_devices:
+ try:
+ indices.append(int(dev.replace('/dev/video', '')))
+ except ValueError:
+ pass
+ if not indices:
+ indices = [0, 1, 2]
+
+ for idx in indices:
+ self.get_logger().info(f'Trying camera index {idx}...')
+ cap = cv2.VideoCapture(idx, cv2.CAP_V4L2)
+ if cap.isOpened():
+ ret, frame = cap.read()
+ if ret and frame is not None:
+ self.get_logger().info(
+ f'Camera {idx} opened successfully: {frame.shape[1]}x{frame.shape[0]}')
+ return cap
+ cap.release()
+ self.get_logger().warn(f'Camera {idx} opened but cannot read frames')
+ else:
+ self.get_logger().warn(f'Camera {idx} failed to open')
+
+ for idx in indices:
+ self.get_logger().info(f'Trying camera index {idx} with default backend...')
+ cap = cv2.VideoCapture(idx)
+ if cap.isOpened():
+ time.sleep(0.5)
+ ret, frame = cap.read()
+ if ret and frame is not None:
+ self.get_logger().info(
+ f'Camera {idx} opened (default backend): {frame.shape[1]}x{frame.shape[0]}')
+ return cap
+ cap.release()
+
+ raise RuntimeError(
+ f'No working camera found. Tried indices {indices}. '
+ f'Devices: {video_devices}')
+
+ def publish_frame(self):
+ ret, frame = self.cap.read()
+ if not ret or frame is None:
+ return
+ msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
+ msg.header.stamp = self.get_clock().now().to_msg()
+ self.publisher.publish(msg)
+
+ def destroy_node(self):
+ if hasattr(self, 'cap') and self.cap is not None:
+ self.cap.release()
+ super().destroy_node()
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = SimpleCameraNode()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/cmr_arm_arucos/launch/aruco_detection.launch.py b/src/cmr_arm_arucos/launch/aruco_detection.launch.py
new file mode 100644
index 00000000..4b2765c8
--- /dev/null
+++ b/src/cmr_arm_arucos/launch/aruco_detection.launch.py
@@ -0,0 +1,48 @@
+"""
+Launch file for aruco detection with a simple OpenCV camera.
+No ZED SDK required -- uses cv2.VideoCapture for the camera feed.
+
+Usage:
+ ros2 launch cmr_arm_arucos aruco_detection.launch.py
+ ros2 launch cmr_arm_arucos aruco_detection.launch.py camera_index:=1 fps:=15
+"""
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+ camera_index_arg = DeclareLaunchArgument(
+ 'camera_index', default_value='0',
+ description='Camera device index (0 for default USB camera)')
+
+ fps_arg = DeclareLaunchArgument(
+ 'fps', default_value='10.0',
+ description='Camera capture FPS')
+
+ camera_node = Node(
+ package='cmr_arm_arucos',
+ executable='simple_camera_node',
+ name='simple_camera_node',
+ output='screen',
+ parameters=[{
+ 'camera_index': LaunchConfiguration('camera_index'),
+ 'fps': LaunchConfiguration('fps'),
+ 'image_topic': '/zed/image_left',
+ }],
+ )
+
+ aruco_node = Node(
+ package='cmr_arm_arucos',
+ executable='arm_aruco_detection',
+ name='aruco_detection_node',
+ output='screen',
+ )
+
+ return LaunchDescription([
+ camera_index_arg,
+ fps_arg,
+ camera_node,
+ aruco_node,
+ ])
diff --git a/src/cmr_arm_arucos/package.xml b/src/cmr_arm_arucos/package.xml
index e9a64d2e..e55d9e8e 100644
--- a/src/cmr_arm_arucos/package.xml
+++ b/src/cmr_arm_arucos/package.xml
@@ -9,6 +9,8 @@
rclpy
sensor_msgs
+ std_msgs
+ cv_bridge
ament_python
diff --git a/src/cmr_arm_arucos/setup.py b/src/cmr_arm_arucos/setup.py
index 22b8f66e..691aafbf 100644
--- a/src/cmr_arm_arucos/setup.py
+++ b/src/cmr_arm_arucos/setup.py
@@ -10,6 +10,7 @@
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
+ ('share/' + package_name + '/launch', ['launch/aruco_detection.launch.py']),
],
install_requires=['setuptools'],
zip_safe=True,
@@ -21,7 +22,8 @@
entry_points={
'console_scripts': [
# PHOBOS_APPEND
- 'arm_aruco_detection = cmr_arm_arucos.aruco_detection_node:main'
+ 'arm_aruco_detection = cmr_arm_arucos.aruco_detection_node:main',
+ 'simple_camera_node = cmr_arm_arucos.simple_camera_node:main'
],
},
)
diff --git a/src/cmr_cams/cmr_cams/object_detection.py b/src/cmr_cams/cmr_cams/object_detection.py
index ee0a4b1a..43fc93ab 100644
--- a/src/cmr_cams/cmr_cams/object_detection.py
+++ b/src/cmr_cams/cmr_cams/object_detection.py
@@ -35,7 +35,7 @@ def __init__(self):
# Parameters
self.declare_parameter('conf_threshold', 0.25)
self.declare_parameter('max_det', 100)
- self.declare_parameter('model_file', 'best.pt')
+ self.declare_parameter('model_file', 'urc_objects_v9.pt')
self.declare_parameter('process_every_n_frames', 5)
conf_threshold = self.get_parameter('conf_threshold').value
diff --git a/src/cmr_cams/config/.gitignore b/src/cmr_cams/config/.gitignore
index c458305b..0904bec5 100644
--- a/src/cmr_cams/config/.gitignore
+++ b/src/cmr_cams/config/.gitignore
@@ -3,3 +3,5 @@
*.pt
!urc_objects_v4.pt
!urc_objects_v7.pt
+!urc_objects_v8.pt
+!urc_objects_v9.pt
diff --git a/src/cmr_cams/config/urc_objects_v8.pt b/src/cmr_cams/config/urc_objects_v8.pt
new file mode 100644
index 00000000..fc0a8aff
Binary files /dev/null and b/src/cmr_cams/config/urc_objects_v8.pt differ
diff --git a/src/cmr_cams/config/urc_objects_v9.pt b/src/cmr_cams/config/urc_objects_v9.pt
new file mode 100644
index 00000000..d7e26226
Binary files /dev/null and b/src/cmr_cams/config/urc_objects_v9.pt differ
diff --git a/src/cmr_cams/launch/object_detection.launch.py b/src/cmr_cams/launch/object_detection.launch.py
index 037898e2..b0f177a1 100644
--- a/src/cmr_cams/launch/object_detection.launch.py
+++ b/src/cmr_cams/launch/object_detection.launch.py
@@ -21,7 +21,7 @@ def generate_launch_description():
model_file_arg = DeclareLaunchArgument(
'model_file',
- default_value='urc_objects_v7.pt',
+ default_value='urc_objects_v9.pt',
description='YOLO model filename in config directory'
)
diff --git a/test_aruco_local.py b/test_aruco_local.py
new file mode 100644
index 00000000..7bc6db62
--- /dev/null
+++ b/test_aruco_local.py
@@ -0,0 +1,145 @@
+"""
+Local webcam test for the aruco detection + pose estimation pipeline.
+Exercises the same logic as aruco_detection_node.py without requiring ROS.
+
+Press 'q' to quit.
+"""
+import cv2
+import numpy as np
+import math
+import time
+
+camera_matrix = np.array([[657.70933821, 0.0, 605.53598505],
+ [0.0, 657.27652417, 343.9524918],
+ [0.0, 0.0, 1.0]])
+dist_coeffs = np.array([0.34688736, 0.07662388, 0.14965771, 0.01600403])
+
+marker_length = 0.1
+half = marker_length / 2.0
+marker_obj_points = np.array([
+ [-half, half, 0],
+ [ half, half, 0],
+ [ half, -half, 0],
+ [-half, -half, 0],
+], dtype=np.float32)
+
+EXPECTED_IDS = {0, 1, 2, 3}
+
+def compute_roll_pitch_yaw_from_normal(normal):
+ normal = normal / np.linalg.norm(normal)
+ nx, ny, nz = normal
+ pitch = math.degrees(np.arcsin(np.clip(-nx, -1.0, 1.0)))
+ yaw = math.degrees(np.arctan2(ny, nz))
+ roll = 0.0
+ return roll, pitch, yaw
+
+def detect_and_annotate(image):
+ if image is None or image.size == 0:
+ return image, {}
+
+ if image.shape[-1] == 4:
+ image = cv2.cvtColor(image, cv2.COLOR_RGBA2BGR)
+
+ aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_50)
+ parameters = cv2.aruco.DetectorParameters()
+ detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)
+ gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+
+ corners, ids, _ = detector.detectMarkers(gray)
+
+ detections = {}
+ if ids is not None:
+ cv2.aruco.drawDetectedMarkers(image, corners, ids)
+
+ for i in range(len(ids)):
+ marker_id = ids[i][0]
+ marker_corners = corners[i][0]
+ cx = int(np.mean(marker_corners[:, 0]))
+ cy = int(np.mean(marker_corners[:, 1]))
+ detections[marker_id] = (cx, cy)
+
+ success, rvec, tvec = cv2.solvePnP(
+ marker_obj_points, corners[i][0],
+ camera_matrix, dist_coeffs)
+ if success:
+ cv2.drawFrameAxes(image, camera_matrix, dist_coeffs, rvec, tvec, 0.1)
+
+ rot_matrix, _ = cv2.Rodrigues(rvec)
+ normal = rot_matrix[:, 2]
+ roll, pitch, yaw = compute_roll_pitch_yaw_from_normal(normal)
+
+ label = f"ID:{marker_id} Y:{yaw:.1f} P:{pitch:.1f}"
+ cv2.putText(image, label, (cx - 40, cy - 20),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+
+ found = set(int(i) for i in ids.flatten())
+ missing = EXPECTED_IDS - found
+ if missing:
+ status = f"Missing markers: {sorted(missing)}"
+ color = (0, 0, 255)
+ else:
+ status = "All 4 markers detected!"
+ color = (0, 255, 0)
+ else:
+ status = "No markers detected"
+ color = (0, 0, 255)
+
+ cv2.putText(image, status, (10, 30),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)
+
+ return image, detections
+
+def main():
+ cap = cv2.VideoCapture(0)
+ if not cap.isOpened():
+ print("ERROR: Could not open webcam index 0. Trying index 1...")
+ cap = cv2.VideoCapture(1)
+ if not cap.isOpened():
+ print("ERROR: No webcam found.")
+ return
+
+ # macOS AVFoundation needs time; first few reads often fail
+ print("Waiting for camera to warm up...")
+ for _ in range(30):
+ ret, _ = cap.read()
+ if ret:
+ break
+ time.sleep(0.2)
+
+ print("Webcam opened. Showing aruco detection. Press 'q' to quit.")
+ print(f"Looking for DICT_6X6_50 markers with IDs: {sorted(EXPECTED_IDS)}")
+ print(f"Camera matrix (hardcoded for ZED -- angles may differ on laptop cam):")
+ print(f" fx={camera_matrix[0,0]:.1f} fy={camera_matrix[1,1]:.1f}")
+ print(f" cx={camera_matrix[0,2]:.1f} cy={camera_matrix[1,2]:.1f}")
+ print()
+
+ consecutive_failures = 0
+ while True:
+ ret, frame = cap.read()
+ if not ret:
+ consecutive_failures += 1
+ if consecutive_failures > 30:
+ print("Too many consecutive frame failures. Exiting.")
+ break
+ time.sleep(0.1)
+ continue
+ consecutive_failures = 0
+
+ annotated, detections = detect_and_annotate(frame)
+
+ if detections:
+ for mid, (cx, cy) in sorted(detections.items()):
+ print(f" Marker {mid}: center=({cx}, {cy})", end="")
+ print()
+
+ cv2.imshow("Aruco Detection Test", annotated)
+
+ if cv2.waitKey(1) & 0xFF == ord('q'):
+ break
+
+ cap.release()
+ cv2.destroyAllWindows()
+ print("Done.")
+
+if __name__ == '__main__':
+ main()