aruco_detector: kommentiert

README.md

             int64[] marker_ids
             geometry_msgs/Pose[] poses
 
-    The array `marker_ids` contains all detected id of the ArUco Markers. The array `poses` contains all poses of the detected ArUco Markers. This message will be published to the topic . 
+    The array `marker_ids` contains all detected ids of the ArUco Markers. The array `poses` contains all poses of the detected ArUco Markers. This message will be published to the topic . 
 
 - topic: `/aruco/aruco_output_images/compressed`
 

src/aruco_detector/aruco_detector/aruco_detector_node.py

+#################################################################################
+# author:       Friegel, Kowatsch, Brueckner
+# date:         27.02.2022
+# description:  With this ROS2-Node can be determined the pose of different ArUco Markers.
+# 
+# 
+# Node parameters: 
+# - marker_size:            
+#           size of the marker in meters
+#           default: 0.1m (10cm)
+#
+# - aruco_dictionary_id:    
+#           Kind of ArUco Marker which is used. For instance DICT_4X4_50, DICT4X4_100, ... 
+#           For furhter information take a look into https://docs.opencv.org/3.4/d9/d6a/group__aruco.html
+#           default: DICT_6X6_100 
+#
+# - input_image_topic
+#           Topic with the pictures. It must be a compressed Image.
+#           default: "/image_raw/compressed"
+#
+# - url_yaml_file
+#           Url where the yaml_file is stored. You get a yaml-file after camera calibration. 
+#           default: "file:///home/ros2/dev2_ws/src/aruco_detector/hd_pro_webcam_c920.yaml"
+#
+#
+# Outputs: 
+# - topic:  /aruco/aruco_poses
+#           The aruco_detector_node publishes a custom ros2 message.
+# 
+# - topic:  /aruco/aruco_output_images/compressed
+#           To this topic will be published the output images. 
+#           In this images the detected ArCuo Marker is highlighted and the pose is drawn using a coordinate system.
+#
+#################################################################################
+
+
+# Imports
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import qos_profile_sensor_data
 
 
 
-########################################################
-# Node parameters
-# - marker_size:            
-#       size of the marker in meters
-#       default: 0.1m (10cm)
-#
-# - aruco_dictionary_id:    
-#       Kind of ArUco Marker which is used. For instance DICT_4X4_50, DICT4X4_100, ... 
-#       For furhter information take a look into https://docs.opencv.org/3.4/d9/d6a/group__aruco.html
-#       default: DICT_6X6_100 
-#
-# - input_image_topic
-#       Topic with the pictures. It must be a compressed Image.
-#       default: "/image_raw/compressed"
-#
-# - url_yaml_file
-#       Url where the yaml_file is stored. You get a yaml-file after camera calibration. 
-#       default: "file:///home/ros2/dev2_ws/src/aruco_detector/hd_pro_webcam_c920.yaml"
-########################################################
 
 class ArucoDetectorNode(rclpy.node.Node):
     def __init__(self):
         super().__init__('aruco_detector_node')
 
- 
         # Declare parameters
         self.declare_parameter("marker_size", .1)                                                        
         self.declare_parameter("aruco_dictionary_id", "DICT_6X6_100")
         camera_info_msg = yaml_handling.yaml_to_CameraInfo(url_yaml_file)
         if camera_info_msg is None:
             self.get_logger().warn("No valid camera info inside the yaml_file!")
-            return
-            
-
+            return          
+        # read and store camera parameters
         self.intrinsic_mat = np.reshape(np.array(camera_info_msg.k), (3, 3))
         self.distortion = np.array(camera_info_msg.d)
 
+        # get the dictionary / parameters
         self.aruco_dictionary = cv2.aruco.Dictionary_get(dictionary_id)
         self.aruco_parameters = cv2.aruco.DetectorParameters_create()
+
+        # Initialize the CvBridge to convert ROS Image messages to OpenCV images
+        # For detailed informations: 
+        # http://docs.ros.org/en/lunar/api/cv_bridge/html/python/index.html
         self.bridge = CvBridge()
 
     def image_callback(self, img_msg):
 
             # iterate through all founded marker_ids and compute their poses
             for i, marker_id in enumerate(marker_ids):
+                # store the calulated current position (translation vector)
                 pose = Pose() 
                 pose.position.x = tvecs[i][0][0]
                 pose.position.y = tvecs[i][0][1]
                 pose.position.z = tvecs[i][0][2]
 
+                # create the rotation matrix 
                 rot_matrix = np.eye(4)
                 rot_matrix[0:3, 0:3] = cv2.Rodrigues(np.array(rvecs[i][0]))[0]
                 
-                angle = math.acos(rot_matrix[0][0])*(180/math.pi)
-
+                # calculate the current angle
                 angle_psi, angle_theta, angle_phi = mat2euler(rot_matrix)
 
+                # calculate the quaternion
+                # rotation axis: z => 2D detection 
+                # If 3D detection is required, the calculation can be adjusted here. 
                 pose.orientation.x = 0.0 
                 pose.orientation.y = 0.0 
                 pose.orientation.z = math.sin((angle_phi) / 2)      
                 pose.orientation.w = math.cos((angle_phi) / 2) 
 
+                # store the calculated pose and marker id in the output list
                 pose_array.poses.append(pose)
                 pose_array.marker_ids.append(marker_id[0])
 
-            # publish the pose array when all markers are detected
+            # publish the pose list when all markers are detected
             self.publisher_marker_poses.publish(pose_array)
 
             # Draw the Axis into the output picture
 
         # Draw a rectangle around the detected markers to highlight them. 
         output_img = cv2.aruco.drawDetectedMarkers(cv_image, corners, marker_ids)  
+        # Convert the cv-image to CompressedImage
         output_img_compressed = self.bridge.cv2_to_compressed_imgmsg(output_img, dst_format='jpg')
 
         # Publish the finished picture with the highlighted Markers and Axis