Feature/aruco sim

src/description/models/marker0/model-1_4.sdf

@@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="Marker0">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://marker0/meshes/Marker0.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>

src/description/models/marker0/model-1_5.sdf

@@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="Marker0">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://marker0/meshes/Marker0.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>

src/description/models/marker0/model.config

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Marker0</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+  <sdf version="1.5">model-1_5.sdf</sdf>
+  <sdf version="1.4">model-1_4.sdf</sdf>
+
+  <author>
+    <name>Mikael Arguedas</name>
+    <email>mikael.arguedas@gmail.com</email>
+  </author>
+
+  <description>
+    ArUco Marker 0 (4x4), generated with code from https://github.com/AD-lite24/aruco_in_gazebo.
+    Modified by Abhinav Kota (abhinav.kota06@gmail.com)
+  </description>
+
+</model>

src/description/models/marker0/model.sdf

@@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="Marker0">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://marker0/meshes/Marker0.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>

src/description/urdf/athena_drive.urdf.xacro

@@ -16,6 +16,7 @@
   <xacro:include filename="gps.xacro"/>
   <xacro:include filename="ground_truth_odom.xacro"/>
   <xacro:include filename="depth_camera.xacro"/> 
+  <xacro:include filename="camera.xacro"/>
 
 
   <!-- Load robot's macro with parameters -->
@@ -54,6 +55,10 @@
       <origin xyz="0.0 0.0 0.1" rpy="0 0 0"/>
   </xacro:depth_camera>
 
+  <xacro:camera parent="base_footprint">
+    <origin xyz="0.0 0.0 0.15" rpy="0 0 0"/>
+  </xacro:camera>
+
   <xacro:if value="$(arg use_mock_hardware)">
     <xacro:athena_drive_ros2_control
     name="athena_drive"

src/description/urdf/camera.xacro

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="camera" params="parent *origin">
+    <link name="camera_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.025 0.09 0.025"/>
+        </geometry>
+        <material name="camera_material">
+          <color rgba="0.1 0.1 0.1 1.0"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.025 0.09 0.025"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name="camera_optical_frame"/>
+
+    <joint name="camera_joint" type="fixed">
+      <xacro:insert_block name="origin"/>
+      <parent link="${parent}"/>
+      <child link="camera_link"/>
+    </joint>
+
+    <joint name="camera_optical_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
+      <parent link="camera_link"/>
+      <child link="camera_optical_frame"/>
+    </joint>
+
+    <gazebo reference="camera_link">
+      <sensor name="camera" type="camera">
+        <always_on>1</always_on>
+        <update_rate>10</update_rate>
+        <visualize>true</visualize>
+        <topic>camera</topic>
+        <camera name="camera">
+          <horizontal_fov>1.05</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>10.0</far>
+          </clip>
+        </camera>
+      </sensor>
+    </gazebo>
+
+  </xacro:macro>
+</robot>

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"

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

src/subsystems/navigation/aruco_detection/aruco_detection/aruco_node.py

@@ -4,63 +4,95 @@
 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)
 
         # Process image (ArUco detection, etc.)
         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)

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):