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 0000000..d783a7b
Binary files /dev/null and b/src/description/models/marker0/materials/textures/Marker0.png differ
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..35a70a6
--- /dev/null
+++ b/src/description/models/marker0/model.config
@@ -0,0 +1,20 @@
+
+
+
+ Marker0
+ 1.0
+ model.sdf
+ model-1_5.sdf
+ model-1_4.sdf
+
+
+ Mikael Arguedas
+ mikael.arguedas@gmail.com
+
+
+
+ ArUco Marker 0 (4x4), generated with code from https://github.com/AD-lite24/aruco_in_gazebo.
+ Modified by Abhinav Kota (abhinav.kota06@gmail.com)
+
+
+
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
+
+
+
+
+
+
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
+
+
+
+
+
+
+
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"
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
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..1c59893 100644
--- a/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py
+++ b/src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py
@@ -4,40 +4,53 @@
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):
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)
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):