Converted C++ aruco node/package to python

requirements.txt

@@ -2,4 +2,5 @@ pygame
 catkin_pkg
 empy==3.3.4
 lark
-numpy
+numpy<2.0
+opencv-contrib-python==4.10.0.84

src/subsystems/navigation/aruco_bt/aruco_bt/aruco_node.py

@@ -0,0 +1,172 @@
+import numpy as np
+import cv2
+import cv2.aruco
+
+import rclpy
+from rclpy.node import Node
+from rclpy.duration import Duration
+from rclpy.time import Time
+from sensor_msgs.msg import Image, CameraInfo
+from geometry_msgs.msg import PoseStamped
+from vision_msgs.msg import Detection2D, BoundingBox2D, ObjectHypothesisWithPose
+from cv_bridge import CvBridge
+import tf2_ros
+import tf2_geometry_msgs
+
+
+class ArUcoNode(Node):
+    def __init__(self):
+        super().__init__('aruco_node')
+
+        self.declare_parameter('marker_size', 0.2)
+        self.declare_parameter('image_topic', '/zed/zed_node/left/image_rect_color')
+        self.declare_parameter('camera_info_topic', '/zed/zed_node/left/camera_info')
+
+        sim = self.get_parameter('use_sim_time').get_parameter_value().bool_value
+        self.marker_size_ = self.get_parameter('marker_size').get_parameter_value().double_value
+        image_topic = self.get_parameter('image_topic').get_parameter_value().string_value
+        camera_info_topic = self.get_parameter('camera_info_topic').get_parameter_value().string_value
+
+        if sim:
+            self.marker_size_ = 0.50
+
+        self.get_logger().info(f'sim={"true" if sim else "false"}')
+        self.get_logger().info(f'marker_size={self.marker_size_:.3f} meters')
+        self.get_logger().info(f'Subscribing to image feed: {image_topic}')
+        self.get_logger().info(f'Subscribing to camera info: {camera_info_topic}')
+
+        aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
+        aruco_params = cv2.aruco.DetectorParameters()
+        self.aruco_detector_ = cv2.aruco.ArucoDetector(aruco_dict, aruco_params)
+
+        self.tf_buffer_ = tf2_ros.Buffer()
+        self.tf_listener_ = tf2_ros.TransformListener(self.tf_buffer_, self)
+
+        self.bridge_ = CvBridge()
+
+        self.image_sub_ = self.create_subscription(
+            Image, image_topic, self.image_callback, 1)
+
+        self.camera_info_sub_ = self.create_subscription(
+            CameraInfo, camera_info_topic, self.camera_info_callback, 10)
+
+        self.detection_pub_ = self.create_publisher(Detection2D, '/aruco_detection', 10)
+
+        self.marker_id_ = -1
+        self.corners_ = []
+        self.all_corners_ = []
+        self.camera_info_received_ = False
+        self.camera_matrix_ = None
+        self.dist_coeffs_ = None
+        self.rvec_ = None
+        self.tvec_ = None
+
+    def camera_info_callback(self, msg):
+        if not self.camera_info_received_:
+            self.camera_matrix_ = np.array(msg.k, dtype=np.float64).reshape(3, 3)
+            self.dist_coeffs_ = np.array(msg.d, dtype=np.float64).reshape(-1, 1)
+            self.camera_info_received_ = True
+            self.get_logger().info('Camera info received')
+            self.destroy_subscription(self.camera_info_sub_)
+            self.camera_info_sub_ = None
+
+    def image_callback(self, msg):
+        try:
+            cv_ptr = self.bridge_.imgmsg_to_cv2(msg, 'mono8')
+        except Exception as e:
+            self.get_logger().error(f'cv_bridge exception: {e}')
+            return
+
+        self.detect_aruco_markers(cv_ptr)
+
+        if self.marker_id_ >= 0 and len(self.corners_) > 0 and self.camera_info_received_:
+            pose = self.estimate_pose(msg)
+            if pose is not None:
+                xs = [pt[0] for pt in self.corners_]
+                ys = [pt[1] for pt in self.corners_]
+
+                detection = Detection2D()
+                detection.header.stamp = self.get_clock().now().to_msg()
+                detection.header.frame_id = 'map'
+                detection.bbox.center.position.x = float(sum(xs) / 4)
+                detection.bbox.center.position.y = float(sum(ys) / 4)
+                detection.bbox.size_x = float(max(xs) - min(xs))
+                detection.bbox.size_y = float(max(ys) - min(ys))
+
+                hypothesis = ObjectHypothesisWithPose()
+                hypothesis.hypothesis.class_id = str(self.marker_id_)
+                hypothesis.hypothesis.score = 1.0
+                hypothesis.pose.pose = pose
+                detection.results.append(hypothesis)
+
+                self.detection_pub_.publish(detection)
+
+    def detect_aruco_markers(self, frame):
+        corners, ids, _ = self.aruco_detector_.detectMarkers(frame)
+
+        if ids is not None and len(ids) > 0:
+            self.marker_id_ = int(ids[0][0])
+            self.all_corners_ = [corners[0]]
+            self.corners_ = [pt for pt in corners[0][0]]
+        else:
+            self.marker_id_ = -1
+            self.corners_ = []
+            self.all_corners_ = []
+
+    def estimate_pose(self, msg):
+        if len(self.all_corners_) == 0:
+            return None
+
+        if self.marker_size_ <= 0:
+            self.get_logger().warn(f'Invalid marker_size_: {self.marker_size_:.3f}')
+            return None
+
+        half = self.marker_size_ / 2.0
+        obj_points = np.array([[-half,  half, 0],[ half,  half, 0],[ half, -half, 0],[-half, -half, 0]], dtype=np.float32)
+
+        img_points = self.all_corners_[0].reshape(4, 2)
+        _, self.rvec_, self.tvec_ = cv2.solvePnP(
+            obj_points, img_points,
+            self.camera_matrix_, self.dist_coeffs_,
+            flags=cv2.SOLVEPNP_IPPE_SQUARE)
+
+        pose_camera = PoseStamped()
+        pose_camera.header.frame_id = msg.header.frame_id
+
+        pose_camera.pose.position.x = float(self.tvec_[0][0])
+        pose_camera.pose.position.y = float(self.tvec_[1][0])
+        pose_camera.pose.position.z = float(self.tvec_[2][0])
+
+        pose_camera.pose.orientation.x = 0.0
+        pose_camera.pose.orientation.y = 0.0
+        pose_camera.pose.orientation.z = 0.0
+        pose_camera.pose.orientation.w = 1.0
+
+        try:
+            pose_camera.header.stamp = Time().to_msg()
+            pose_map = self.tf_buffer_.transform(pose_camera, 'map', timeout=Duration(seconds=0.1))
+
+            tf_map_base = self.tf_buffer_.lookup_transform(
+                'map', 'base_footprint', Time(),
+                timeout=Duration(seconds=0.1))
+
+            pose_map.pose.orientation.x = tf_map_base.transform.rotation.x
+            pose_map.pose.orientation.y = tf_map_base.transform.rotation.y
+            pose_map.pose.orientation.z = tf_map_base.transform.rotation.z
+            pose_map.pose.orientation.w = tf_map_base.transform.rotation.w
+
+            return pose_map.pose
+        except Exception as ex:
+            self.get_logger().warn(f'Could not transform to map frame: {ex}')
+            return None
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ArUcoNode()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/subsystems/navigation/aruco_bt/launch/aruco.launch.py

@@ -9,6 +9,21 @@
         default_value='0.2',
         description='Size of ArUco marker in meters'
     ),
+    DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation clock if true'
+    ),
+    DeclareLaunchArgument(
+        'image_topic',
+        default_value='/zed/zed_node/left/image_rect_color',
+        description='Image topic to subscribe to for ArUco detection'
+    ),
+    DeclareLaunchArgument(
+        'camera_info_topic',
+        default_value='/zed/zed_node/left/camera_info',
+        description='Camera info topic corresponding to the image topic'
+    ),
 ]
 
 def generate_launch_description():
@@ -18,7 +33,9 @@ def generate_launch_description():
         name='aruco_node',
         parameters=[
             {'use_sim_time': LaunchConfiguration('use_sim_time')},
-            {'marker_size': LaunchConfiguration('marker_size')}
+            {'marker_size': LaunchConfiguration('marker_size')},
+            {'image_topic': LaunchConfiguration('image_topic')},
+            {'camera_info_topic': LaunchConfiguration('camera_info_topic')},
         ],
         output='screen'
     )

src/subsystems/navigation/aruco_bt/package.xml

@@ -3,25 +3,26 @@
 <package format="3">
   <name>aruco_bt</name>
   <version>0.0.0</version>
-  <description>C++ package for aruco detection and pose estimation</description>
+  <description>Python package for aruco detection and pose estimation</description>
   <maintainer email="abhinav.kota06@gmail.com">abhinavkota06</maintainer>
   <license>TODO: License declaration</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_python</buildtool_depend>
 
-  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>vision_msgs</depend>
   <depend>cv_bridge</depend>
-  <depend>libopencv-dev</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
 
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
 
   <export>
-    <build_type>ament_cmake</build_type>
+    <build_type>ament_python</build_type>
   </export>
 </package>

src/subsystems/navigation/aruco_bt/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/aruco_bt
+[install]
+install_scripts=$base/lib/aruco_bt

src/subsystems/navigation/aruco_bt/setup.py

@@ -0,0 +1,28 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'aruco_bt'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='abhinavkota06',
+    maintainer_email='abhinav.kota06@gmail.com',
+    description='Python package for aruco detection and pose estimation',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'aruco_node = aruco_bt.aruco_node:main',
+        ],
+    },
+)