IoT_Furniture_Localization/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
from cv_bridge import CvBridge

import cv2

import numpy as np
import math
from transforms3d.euler import mat2euler

from aruco_detector import yaml_handling 

from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Pose
from aruco_interfaces.msg import ArucoMarkerPose




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")
        self.declare_parameter("input_image_topic", "/image_raw/compressed")                 
        self.declare_parameter("url_yaml_file", "file:///home/ros2/dev2_ws/src/aruco_detector/hd_pro_webcam_c920.yaml")      # url to yaml file (calibration file) 
        # Read parameters
        self.marker_size    = self.get_parameter("marker_size").get_parameter_value().double_value
        dictionary_id_name  = self.get_parameter("aruco_dictionary_id").get_parameter_value().string_value
        input_image_topic   = self.get_parameter("input_image_topic").get_parameter_value().string_value
        url_yaml_file       = self.get_parameter("url_yaml_file").get_parameter_value().string_value
        
        # check dictionary_id 
        try:
            dictionary_id = cv2.aruco.__getattribute__(dictionary_id_name)
            if type(dictionary_id) != type(cv2.aruco.DICT_5X5_100):
                raise AttributeError
        except AttributeError:
            self.get_logger().error("bad aruco_dictionary_id: {}".format(dictionary_id_name))
            options = "\n".join([s for s in dir(cv2.aruco) if s.startswith("DICT")])
            self.get_logger().error("valid options: {}".format(options))

        # Set up subscriber 
        # (images from the camera in wich the ArUco Marker will be detected)
        self.create_subscription(CompressedImage, input_image_topic, self.image_callback, qos_profile_sensor_data)    

        # Set up publishers
        # - current poses
        # - images with the detected and highlighted ArUco-Tags
        self.publisher_marker_poses   = self.create_publisher(ArucoMarkerPose, '/aruco/aruco_poses', 10)        
        self.publisher_output_images  = self.create_publisher(CompressedImage, '/aruco/aruco_output_images/compressed', 10)         

        # Load the camera parameters from yaml-file
        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          
        # 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):
        """
        image_callback(self, img_msg)
        Callback function from the image-subscriber. 
        Is called if a new compressed image will be received.   
        """

        # Convert the compressed input image into cv_image
        # This is done, because the folling functions needs the cv_image-type.
        cv_image = self.bridge.compressed_imgmsg_to_cv2(img_msg,desired_encoding='passthrough')


        # Basic marker detection
        # -corners: vector of detected marker corners
        # -marker_ids: ids read from the ArUco Marker
        corners, marker_ids, rejected = cv2.aruco.detectMarkers(cv_image,
                                                                self.aruco_dictionary,
                                                                parameters=self.aruco_parameters)

        #check if marker is detected, then estimate pose an publish
        if marker_ids is not None:

            # Pose estimation for single markers
            # -rvecs: array of output rotation vectors
            # -tvecs: array of output translation vectors
            rvecs, tvecs, _objPoints = cv2.aruco.estimatePoseSingleMarkers(corners,
                                                                           self.marker_size, 
                                                                           self.intrinsic_mat,
                                                                           self.distortion)

            # Create the variable in which all poses and marker_ids are stored. 
            # ArucoMarkerPose is an custom ros2 message, which is found inside the folder "aruco_interfaces"
            pose_array = ArucoMarkerPose()  

            # 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]
                
                # 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 list when all markers are detected
            self.publisher_marker_poses.publish(pose_array)

            # Draw the Axis into the output picture
            output_img = cv2.aruco.drawAxis(cv_image, 
                                            self.intrinsic_mat,
                                            self.distortion, 
                                            rvecs,
                                            tvecs,
                                            0.5
                                        )

        # 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
        self.publisher_output_images.publish(output_img_compressed)


def main(args=None):
    rclpy.init(args=args)
    minimal_publisher = ArucoDetectorNode()
    rclpy.spin(minimal_publisher)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()