diff --git a/README.md b/README.md index 9db10b3..ebc5d67 100644 --- a/README.md +++ b/README.md @@ -44,7 +44,7 @@ An overview about the running topics is illustrated in the next picture. 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` diff --git a/src/aruco_detector/aruco_detector/aruco_detector_node.py b/src/aruco_detector/aruco_detector/aruco_detector_node.py index a933702..c8c1314 100644 --- a/src/aruco_detector/aruco_detector/aruco_detector_node.py +++ b/src/aruco_detector/aruco_detector/aruco_detector_node.py @@ -1,3 +1,40 @@ +################################################################################# +# 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 @@ -17,31 +54,11 @@ from aruco_interfaces.msg import ArucoMarkerPose -######################################################## -# 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") @@ -77,14 +94,18 @@ class ArucoDetectorNode(rclpy.node.Node): 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): @@ -123,27 +144,32 @@ class ArucoDetectorNode(rclpy.node.Node): # 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 @@ -157,6 +183,7 @@ class ArucoDetectorNode(rclpy.node.Node): # 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