From ebeb05a7c205d384f1b262b815015618452d329b Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Fri, 16 Jan 2026 17:33:01 -0500 Subject: [PATCH 1/8] Added Aruco Marker 0 model --- .../marker0/materials/textures/Marker0.png | Bin 0 -> 1183 bytes .../models/marker0/meshes/Marker0.dae | 3 +++ src/description/models/marker0/model-1_4.sdf | 15 ++++++++++++++ src/description/models/marker0/model-1_5.sdf | 15 ++++++++++++++ src/description/models/marker0/model.config | 19 ++++++++++++++++++ src/description/models/marker0/model.sdf | 15 ++++++++++++++ 6 files changed, 67 insertions(+) create mode 100644 src/description/models/marker0/materials/textures/Marker0.png create mode 100644 src/description/models/marker0/meshes/Marker0.dae create mode 100644 src/description/models/marker0/model-1_4.sdf create mode 100644 src/description/models/marker0/model-1_5.sdf create mode 100644 src/description/models/marker0/model.config create mode 100644 src/description/models/marker0/model.sdf diff --git a/src/description/models/marker0/materials/textures/Marker0.png b/src/description/models/marker0/materials/textures/Marker0.png new file mode 100644 index 0000000000000000000000000000000000000000..686090d763530994eb1ba081d32ba777e2e075d2 GIT binary patch literal 1183 zcmeAS@N?(olHy`uVBq!ia0vp^t3a584M^ISU1@Vhj#(;!0?ZfljKA+My4h{n#G?9%_l(&Id9JMVU%!9W%-jF}pW%lrX8ZN~ z&K2_fF;@uD^pOyFb;PGnWEGnl4@YT|nmbo0voKqWmEmON)>Vz1%uaJ>oSf)%^*{^b zLccRlJQijp9BK$qPb*pyU}}))utM0_GH8X=3}%6<9uCt~ zq2*Z)D?;}^&}4QRqFNfS|I_mNbT$94>g4VBrk2c4`nhPC&!Z4tj;*W2ZBiXpXil~W zWp-Noiq&6IAnTNh-_?eI)l~gb~^3d_2Jnbv1PxtSQUXk1!Ya6~Ie7bz~_JHGT zyQ|g(tk<*umbb8vx9n@o!noMFH+P)|%7Xt)k9tG{|D9sp3oLOMJYD@<);T3K0RSOE B1`7ZH literal 0 HcmV?d00001 diff --git a/src/description/models/marker0/meshes/Marker0.dae b/src/description/models/marker0/meshes/Marker0.dae new file mode 100644 index 0000000..6c8252d --- /dev/null +++ b/src/description/models/marker0/meshes/Marker0.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:12d843202591af4ace325c6d3c998c72d31ad3e37f4eb9b5300964c2748e4e8e +size 5637 diff --git a/src/description/models/marker0/model-1_4.sdf b/src/description/models/marker0/model-1_4.sdf new file mode 100644 index 0000000..43a6f7d --- /dev/null +++ b/src/description/models/marker0/model-1_4.sdf @@ -0,0 +1,15 @@ + + + + true + + + + + model://marker0/meshes/Marker0.dae + + + + + + diff --git a/src/description/models/marker0/model-1_5.sdf b/src/description/models/marker0/model-1_5.sdf new file mode 100644 index 0000000..43a6f7d --- /dev/null +++ b/src/description/models/marker0/model-1_5.sdf @@ -0,0 +1,15 @@ + + + + true + + + + + model://marker0/meshes/Marker0.dae + + + + + + diff --git a/src/description/models/marker0/model.config b/src/description/models/marker0/model.config new file mode 100644 index 0000000..29b4338 --- /dev/null +++ b/src/description/models/marker0/model.config @@ -0,0 +1,19 @@ + + + + Marker0 + 1.0 + model.sdf + model-1_5.sdf + model-1_4.sdf + + + UMDLoop + abhinav.kota06@gmail.com + + + + ArUco Marker 0 (4x4) + + + diff --git a/src/description/models/marker0/model.sdf b/src/description/models/marker0/model.sdf new file mode 100644 index 0000000..6566090 --- /dev/null +++ b/src/description/models/marker0/model.sdf @@ -0,0 +1,15 @@ + + + + true + + + + + model://marker0/meshes/Marker0.dae + + + + + + From ba59ae681f614f824c49629138351f44c8102603 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Tue, 20 Jan 2026 23:15:53 -0500 Subject: [PATCH 2/8] Fixed model.config --- src/description/models/marker0/model.config | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/description/models/marker0/model.config b/src/description/models/marker0/model.config index 29b4338..35a70a6 100644 --- a/src/description/models/marker0/model.config +++ b/src/description/models/marker0/model.config @@ -8,12 +8,13 @@ model-1_4.sdf - UMDLoop - abhinav.kota06@gmail.com + Mikael Arguedas + mikael.arguedas@gmail.com - ArUco Marker 0 (4x4) + ArUco Marker 0 (4x4), generated with code from https://github.com/AD-lite24/aruco_in_gazebo. + Modified by Abhinav Kota (abhinav.kota06@gmail.com) From 80e6531906ed7ce32182930fb72892dd697e7fdf Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Tue, 20 Jan 2026 23:16:16 -0500 Subject: [PATCH 3/8] Added normal camera to URDF --- src/description/urdf/athena_drive.urdf.xacro | 5 ++ src/description/urdf/camera.xacro | 59 ++++++++++++++++++++ 2 files changed, 64 insertions(+) create mode 100644 src/description/urdf/camera.xacro diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 9deb819..1949d2c 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -16,6 +16,7 @@ + @@ -54,6 +55,10 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 10 + true + camera + + 1.05 + + 640 + 480 + R8G8B8 + + + 0.1 + 10.0 + + + + + + + From d17ba6176b9204a2cc9155787ff5d2daefce10b5 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Tue, 20 Jan 2026 23:17:29 -0500 Subject: [PATCH 4/8] Add Bounding Box (BB) msg for Aruco detection --- src/msgs/msg/BB.msg | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 src/msgs/msg/BB.msg diff --git a/src/msgs/msg/BB.msg b/src/msgs/msg/BB.msg new file mode 100644 index 0000000..3fd9d85 --- /dev/null +++ b/src/msgs/msg/BB.msg @@ -0,0 +1,10 @@ +int64 img_width +int64 img_height +int64 bb_top_left_x +int64 bb_top_left_y +int64 bb_bottom_right_x +int64 bb_bottom_right_y +int64 bb_top_right_x +int64 bb_top_right_y +int64 bb_bottom_left_x +int64 bb_bottom_left_y \ No newline at end of file From e2cd2d692509b8a458456b4b1dd1911aff7d3d94 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Tue, 20 Jan 2026 23:19:29 -0500 Subject: [PATCH 5/8] Fixed import path for BB msg --- .../navigation/aruco_detection/aruco_detection/aruco_node.py | 2 +- .../aruco_detection/aruco_detection/correction_node.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py b/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py index 4df29cf..94484c3 100644 --- a/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py +++ b/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py @@ -4,7 +4,7 @@ from std_msgs.msg import String from cv_bridge import CvBridge import cv2 -from interfaces.msg import BB +from msgs.msg import BB class ZedArUcoNode(Node): def __init__(self): diff --git a/src/subsystems/navigation/aruco_detection/aruco_detection/correction_node.py b/src/subsystems/navigation/aruco_detection/aruco_detection/correction_node.py index 536d7ca..82579ed 100644 --- a/src/subsystems/navigation/aruco_detection/aruco_detection/correction_node.py +++ b/src/subsystems/navigation/aruco_detection/aruco_detection/correction_node.py @@ -1,7 +1,7 @@ import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist -from interfaces.msg import BB +from msgs.msg import BB class CorrectionNode(Node): def __init__(self): From 62b9c0378e487ac2cd9fe946c894c3e1b2a33143 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Wed, 21 Jan 2026 13:27:05 -0500 Subject: [PATCH 6/8] Added BB to CMakeLists so the interface is generated --- src/msgs/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 2b9df55..b94b61a 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(builtin_interfaces REQUIRED) find_package(action_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/BB.msg" "msg/CANA.msg" "msg/CANB.msg" "msg/Currentdraw.msg" From d73db84acff40aed4121bb7225fbf8d0d7e19367 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Wed, 21 Jan 2026 13:27:35 -0500 Subject: [PATCH 7/8] Fixed ArUco marker image, previous one wasn't 4x4 50 --- .../marker0/materials/textures/Marker0.png | Bin 1183 -> 318 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/src/description/models/marker0/materials/textures/Marker0.png b/src/description/models/marker0/materials/textures/Marker0.png index 686090d763530994eb1ba081d32ba777e2e075d2..d783a7b49c833a80a45d88b0c8c3921cf403ea07 100644 GIT binary patch literal 318 zcmeAS@N?(olHy`uVBq!ia0vp^t3a5A8AzUwIJpr>DF*n2xB}__|Nkc^Cr3v|_w@7} zKYo1KvSs4p;x;xmKYsjp^5ltNOzd)?q8pwrjv*QM-riow-{c_C_Axtc(b>!mt4^*s zcs5HxKtICAWR-Gcr`zg!)vwjf<~#2C7M>5b|CX#=YkffX^7c=9?$IF-aD{WV_08#7 zGq*NgeI*x^KK;%Sfqw?BL(BMJeg-LtaF3$I_=R)53KsJb^`>#NM$C*DY`ef8PF z6$1X|e*Ly>t@P~vu-42ka@Vhj#(;!0?ZfljKA+My4h{n#G?9%_l(&Id9JMVU%!9W%-jF}pW%lrX8ZN~ z&K2_fF;@uD^pOyFb;PGnWEGnl4@YT|nmbo0voKqWmEmON)>Vz1%uaJ>oSf)%^*{^b zLccRlJQijp9BK$qPb*pyU}}))utM0_GH8X=3}%6<9uCt~ zq2*Z)D?;}^&}4QRqFNfS|I_mNbT$94>g4VBrk2c4`nhPC&!Z4tj;*W2ZBiXpXil~W zWp-Noiq&6IAnTNh-_?eI)l~gb~^3d_2Jnbv1PxtSQUXk1!Ya6~Ie7bz~_JHGT zyQ|g(tk<*umbb8vx9n@o!noMFH+P)|%7Xt)k9tG{|D9sp3oLOMJYD@<);T3K0RSOE B1`7ZH From 25508bfbf41e9fd3a1c70d92cff540f476cdd338 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Wed, 21 Jan 2026 15:14:32 -0500 Subject: [PATCH 8/8] Fixed aruco_node OpenCV version issues, added support for sim parameter --- .../aruco_detection/aruco_node.py | 85 +++++++++++++------ 1 file changed, 58 insertions(+), 27 deletions(-) diff --git a/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py b/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py index 94484c3..1c59893 100644 --- a/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py +++ b/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py @@ -10,34 +10,47 @@ class ZedArUcoNode(Node): def __init__(self): super().__init__('zed_aruco_node') + self.declare_parameter("sim", False) + self.sim = self.get_parameter("sim").get_parameter_value().bool_value + + if self.sim: + image_topic = '/camera' + depth_topic = '/depth_camera' + else: + image_topic = '/zed/zed_node/left_gray/image_rect_gray' + depth_topic = '/zed/zed_node/depth/depth_registered' + + self.get_logger().info(f"sim={self.sim}") + self.get_logger().info(f"Subscribing to image feed: {image_topic}") + self.get_logger().info(f"Subscribing to depth image feed: {depth_topic}") + # Initialize CvBridge to convert ROS images to OpenCV self.bridge = CvBridge() # Subscribe to the ZED camera image and depth topics self.image_sub = self.create_subscription( - Image, '/zed/zed_node/left_gray/image_rect_gray', self.image_callback, 10 + Image, image_topic, self.image_callback, 10 ) self.depth_sub = self.create_subscription( - Image, '/zed/zed_node/depth/depth_registered', self.depth_callback, 10 + Image, depth_topic, self.depth_callback, 10 ) - self.image_pub = self.create_publisher(Image, 'annotated_img', 10) + self.image_pub = self.create_publisher(Image, 'aruco_annotated_img', 10) self.tag_pub = self.create_publisher(BB, 'aruco_loc', 10) + self.depth_pub = self.create_publisher(String, 'aruco_depth', 10) - self.depth_pub = self.create_publisher(String, 'depth', 10) - - #Load the ArUco dictionary - self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) - self.aruco_params = cv2.aruco.DetectorParameters() - self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params) + try: + self.aruco_params = cv2.aruco.DetectorParameters() + except AttributeError: + self.aruco_params = cv2.aruco.DetectorParameters_create() self.corners = None self.marker_id = None self.latest_depth_map = None def image_callback(self, msg): - # Convert ROS Image to OpenCV Image frame = self.bridge.imgmsg_to_cv2(msg, "mono8") frame_color = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) @@ -45,22 +58,41 @@ def image_callback(self, msg): self.detect_aruco_markers(frame) # If we have a valid marker and a valid depth map, publish the information - if self.marker_id is not None and self.latest_depth_map is not None: + if self.marker_id is not None: for i in range(4): start_point = tuple(map(int, self.corners[i])) end_point = tuple(map(int, self.corners[(i + 1) % 4])) # Connect to next corner cv2.line(frame_color, start_point, end_point, (0, 255, 0), 2) # Draw green lines - - - depth_values = [ - self.latest_depth_map[int(self.corners[0][1]), int(self.corners[0][0])], # Top-left - self.latest_depth_map[int(self.corners[1][1]), int(self.corners[1][0])], # Top-right - self.latest_depth_map[int(self.corners[2][1]), int(self.corners[2][0])], # Bottom-right - self.latest_depth_map[int(self.corners[3][1]), int(self.corners[3][0])] # Bottom-left - ] - - depth_avg = sum(depth_values) / len(depth_values) # Compute the average depth + + if self.corners is not None: + text_pos = tuple(map(int, self.corners[0])) + + cv2.putText( + frame_color, + f"id: {self.marker_id}", + (text_pos[0], text_pos[1] - 20), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 0, 255), + 2, + cv2.LINE_AA + ) + + if self.latest_depth_map is not None: + depth_values = [ + self.latest_depth_map[int(self.corners[0][1]), int(self.corners[0][0])], # Top-left + self.latest_depth_map[int(self.corners[1][1]), int(self.corners[1][0])], # Top-right + self.latest_depth_map[int(self.corners[2][1]), int(self.corners[2][0])], # Bottom-right + self.latest_depth_map[int(self.corners[3][1]), int(self.corners[3][0])] # Bottom-left + ] + + depth_avg = sum(depth_values) / len(depth_values) + depth_message = String() + depth_message.data = str(depth_avg) + self.depth_pub.publish(depth_message) + else: + self.get_logger().warn("Depth map not received yet, skipping depth calc") # Publish tag bounding box message = BB() @@ -75,29 +107,28 @@ def image_callback(self, msg): message.bb_top_left_y = min(y_values) message.bb_bottom_right_x = max(x_values) message.bb_bottom_right_y = max(y_values) - - depth_message = String() - depth_message.data = str(depth_avg) - self.depth_pub.publish(depth_message) self.tag_pub.publish(message) annotated_msg = self.bridge.cv2_to_imgmsg(frame_color, encoding="bgr8") + annotated_msg.header = msg.header self.image_pub.publish(annotated_msg) + def depth_callback(self, msg): # Convert ROS Image to depth map self.latest_depth_map = self.bridge.imgmsg_to_cv2(msg, "32FC1") def detect_aruco_markers(self, frame): - - corners, ids, _ = self.aruco_detector.detectMarkers(frame) + corners, ids, _ = cv2.aruco.detectMarkers(frame, self.aruco_dict, parameters=self.aruco_params) + if ids is not None: for i in range(len(ids)): self.marker_id = ids[i][0] self.corners = corners[i][0] else: self.marker_id = None + self.corners = None def main(args=None): rclpy.init(args=args)