upload content

build/aruco_detector/aruco_detector.egg-info/PKG-INFO

@@ -0,0 +1,10 @@
+Metadata-Version: 1.2
+Name: aruco-detector
+Version: 0.0.0
+Summary: Node to detect ArUco-codes
+Home-page: UNKNOWN
+Maintainer: ros2
+Maintainer-email: bruecknerja86834@th-nuernberg.de
+License: TODO: License declaration
+Description: UNKNOWN
+Platform: UNKNOWN

build/aruco_detector/aruco_detector.egg-info/SOURCES.txt

@@ -0,0 +1,18 @@
+package.xml
+setup.cfg
+setup.py
+../../build/aruco_detector/aruco_detector.egg-info/PKG-INFO
+../../build/aruco_detector/aruco_detector.egg-info/SOURCES.txt
+../../build/aruco_detector/aruco_detector.egg-info/dependency_links.txt
+../../build/aruco_detector/aruco_detector.egg-info/entry_points.txt
+../../build/aruco_detector/aruco_detector.egg-info/requires.txt
+../../build/aruco_detector/aruco_detector.egg-info/top_level.txt
+../../build/aruco_detector/aruco_detector.egg-info/zip-safe
+aruco_detector/__init__.py
+aruco_detector/aruco_detector_node.py
+aruco_detector/transformations.py
+aruco_detector/yaml_handling.py
+resource/aruco_detector
+test/test_copyright.py
+test/test_flake8.py
+test/test_pep257.py

build/aruco_detector/aruco_detector.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

build/aruco_detector/aruco_detector.egg-info/entry_points.txt

@@ -0,0 +1,3 @@
+[console_scripts]
+aruco_detector_node = aruco_detector.aruco_detector_node:main
+

build/aruco_detector/aruco_detector.egg-info/requires.txt

@@ -0,0 +1 @@
+setuptools

build/aruco_detector/aruco_detector.egg-info/top_level.txt

@@ -0,0 +1 @@
+aruco_detector

build/aruco_detector/aruco_detector.egg-info/zip-safe

@@ -0,0 +1 @@
+

build/aruco_detector/build/lib/aruco_detector/aruco_detector_node.py

@@ -0,0 +1,146 @@
+from numpy.lib.function_base import _angle_dispatcher
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from cv_bridge import CvBridge
+
+import numpy as np
+import cv2
+
+from transforms3d.quaternions import mat2quat
+from transforms3d.euler import mat2euler
+
+from aruco_detector import yaml_handling 
+
+from sensor_msgs.msg import CompressedImage, Image
+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_name
+        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
+        self.create_subscription(CompressedImage, input_image_topic, self.image_callback, qos_profile_sensor_data)    # subsribes the images from the camera
+
+        # Set up publishers
+        self.publisher_marker_poses   = self.create_publisher(ArucoMarkerPose, '/aruco/aruco_poses', 10)        # publishes the current poses
+        self.publisher_output_images  = self.create_publisher(CompressedImage, '/aruco/aruco_output_images/compressed', 10)          # publishes the images with the detected aruco-tags
+
+        # Load the camera parameters from yaml-file
+        camera_info_msg = yaml_handling.yaml_to_CameraInfo("/home/ros2/dev2_ws/src/aruco_detector/hd_pro_webcam_c920.yaml")#url_yaml_file)
+        if camera_info_msg is None:
+            self.get_logger().warn("No camera info has been received!")
+            return
+            
+        self.intrinsic_mat = np.reshape(np.array(camera_info_msg.k), (3, 3))
+        self.distortion = np.array(camera_info_msg.d)
+
+        self.aruco_dictionary = cv2.aruco.Dictionary_get(dictionary_id)
+        self.aruco_parameters = cv2.aruco.DetectorParameters_create()
+        self.bridge = CvBridge()
+
+    def image_callback(self, img_msg):
+        
+
+        cv_image = self.bridge.compressed_imgmsg_to_cv2(img_msg,desired_encoding='passthrough')
+
+        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:
+            rvecs, tvecs, _objPoints = cv2.aruco.estimatePoseSingleMarkers(corners,
+                                                                           self.marker_size, 
+                                                                           self.intrinsic_mat,
+                                                                           self.distortion)
+
+            #self.get_logger().error("rvecs: {}".format(rvecs))
+            pose_array = ArucoMarkerPose()                                                       
+            for i, marker_id in enumerate(marker_ids):
+                pose = Pose() 
+                pose.position.x = tvecs[i][0][0]
+                pose.position.y = tvecs[i][0][1]
+                pose.position.z = tvecs[i][0][2]
+
+                rot_matrix = np.eye(4)
+                rot_matrix[0:3, 0:3] = cv2.Rodrigues(np.array(rvecs[i][0]))[0]
+                
+
+                import math
+                angle = math.acos(rot_matrix[0][0])*(180/math.pi)
+
+                angle_psi, angle_theta, angle_phi = mat2euler(rot_matrix)
+
+
+                #quat = mat2quat(rot_matrix[0:3, 0:3])
+
+
+                self.get_logger().error("angle_theta: \n{}\n\n".format(angle_theta*(180/math.pi)))
+                self.get_logger().error("angle_phi: \n{}\n\n".format(angle_phi*(180/math.pi)))
+                self.get_logger().error("angle_psi: \n{}\n\n".format(angle_psi*(180/math.pi)))
+
+
+
+                # parameter einfügen für unterscheidung zwischen 2D und 3D
+                # einfache if abfrage
+
+                pose.orientation.x = 0.0 #quat[0]
+                pose.orientation.y = 0.0 #quat[1]
+                pose.orientation.z = math.sin((angle_phi) / 2) #quat[2]       
+                pose.orientation.w = math.cos((angle_phi) / 2) #quat[3]      
+
+                pose_array.poses.append(pose)
+                pose_array.marker_ids.append(marker_id[0])
+
+            self.publisher_marker_poses.publish(pose_array)
+
+            output_img = cv2.aruco.drawAxis(cv_image, 
+                                            self.intrinsic_mat,
+                                            self.distortion, 
+                                            rvecs,
+                                            tvecs,
+                                            0.5
+                                        )
+
+        output_img = cv2.aruco.drawDetectedMarkers(cv_image, corners, marker_ids)  
+        output_img_compressed = self.bridge.cv2_to_compressed_imgmsg(output_img, dst_format='jpg')
+        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()
+
+

build/aruco_detector/build/lib/aruco_detector/yaml_handling.py

@@ -0,0 +1,47 @@
+import yaml
+from sensor_msgs.msg import CameraInfo
+
+
+def yaml_to_CameraInfo(yaml_fname):
+    # Load data from file
+    with open(yaml_fname, "r") as file_handle:
+        calib_data = yaml.load(file_handle)
+
+    # try to load the parameters
+    camera_info_msg = CameraInfo()
+    try: 
+        camera_info_msg.width = calib_data["image_width"]
+    except:
+        pass
+
+    try: 
+        camera_info_msg.height = calib_data["image_height"]
+    except:
+        pass
+
+    try: 
+        camera_info_msg.k = calib_data["camera_matrix"]["data"]
+    except:
+        pass
+
+    try: 
+        camera_info_msg.d = calib_data["distortion_coefficients"]["data"]
+    except:
+        pass
+
+    try: 
+        camera_info_msg.r = calib_data["rectification_matrix"]["data"]
+    except:
+        pass
+
+    try: 
+        camera_info_msg.p = calib_data["projection_matrix"]["data"]
+    except:
+        pass
+
+    try: 
+        camera_info_msg.distortion_model = calib_data["distortion_model"]
+    except:
+        pass
+
+    return camera_info_msg    

build/aruco_detector/colcon_build.rc

@@ -0,0 +1 @@
+0

build/aruco_detector/colcon_command_prefix_setup_py.sh

@@ -0,0 +1 @@
+# generated from colcon_core/shell/template/command_prefix.sh.em

build/aruco_detector/colcon_command_prefix_setup_py.sh.env

@@ -0,0 +1,70 @@
+AMENT_PREFIX_PATH=/home/ros2/dev2_ws/install/v4l2_camera:/home/ros2/dev2_ws/install/turtle_follower_py:/home/ros2/dev2_ws/install/camera_calibration:/home/ros2/dev2_ws/install/aruco_interfaces:/home/ros2/dev2_ws/install/aruco_detector:/opt/ros/foxy
+CMAKE_PREFIX_PATH=/home/ros2/dev2_ws/install/v4l2_camera:/home/ros2/dev2_ws/install/aruco_interfaces
+COLCON=1
+COLCON_PREFIX_PATH=/home/ros2/dev2_ws/install
+COLORTERM=truecolor
+DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/1000/bus
+DESKTOP_SESSION=ubuntu
+DISPLAY=:0
+GDMSESSION=ubuntu
+GJS_DEBUG_OUTPUT=stderr
+GJS_DEBUG_TOPICS=JS ERROR;JS LOG
+GNOME_DESKTOP_SESSION_ID=this-is-deprecated
+GNOME_SHELL_SESSION_MODE=ubuntu
+GNOME_TERMINAL_SCREEN=/org/gnome/Terminal/screen/ed9a88cb_996e_4783_90fb_206aa627c957
+GNOME_TERMINAL_SERVICE=:1.166
+GPG_AGENT_INFO=/run/user/1000/gnupg/S.gpg-agent:0:1
+GTK_MODULES=gail:atk-bridge
+HOME=/home/ros2
+IM_CONFIG_PHASE=1
+INVOCATION_ID=331653316740409188736b6261a239d2
+JOURNAL_STREAM=8:183562
+LANG=en_US.UTF-8
+LC_ADDRESS=de_DE.UTF-8
+LC_ALL=en_US.UTF-8
+LC_IDENTIFICATION=de_DE.UTF-8
+LC_MEASUREMENT=de_DE.UTF-8
+LC_MONETARY=de_DE.UTF-8
+LC_NAME=de_DE.UTF-8
+LC_NUMERIC=de_DE.UTF-8
+LC_PAPER=de_DE.UTF-8
+LC_TELEPHONE=de_DE.UTF-8
+LC_TIME=de_DE.UTF-8
+LD_LIBRARY_PATH=/home/ros2/dev2_ws/install/v4l2_camera/lib:/home/ros2/dev2_ws/install/aruco_interfaces/lib:/opt/ros/foxy/opt/yaml_cpp_vendor/lib:/opt/ros/foxy/opt/rviz_ogre_vendor/lib:/opt/ros/foxy/lib/x86_64-linux-gnu:/opt/ros/foxy/lib
+LESSCLOSE=/usr/bin/lesspipe %s %s
+LESSOPEN=| /usr/bin/lesspipe %s
+LOGNAME=ros2
+LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
+MANAGERPID=7832
+OLDPWD=/home/ros2/dev2_ws/launch
+PAPERSIZE=a4
+PATH=/opt/ros/foxy/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
+PWD=/home/ros2/dev2_ws/build/aruco_detector
+PYTHONPATH=/home/ros2/dev2_ws/install/turtle_follower_py/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/camera_calibration/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/aruco_interfaces/lib/python3.8/site-packages:/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages:/opt/ros/foxy/lib/python3.8/site-packages
+QT_ACCESSIBILITY=1
+QT_IM_MODULE=ibus
+ROS_DISTRO=foxy
+ROS_LOCALHOST_ONLY=0
+ROS_PYTHON_VERSION=3
+ROS_VERSION=2
+SESSION_MANAGER=local/ubuntu:@/tmp/.ICE-unix/8042,unix/ubuntu:/tmp/.ICE-unix/8042
+SHELL=/bin/bash
+SHLVL=1
+SSH_AGENT_PID=8007
+SSH_AUTH_SOCK=/run/user/1000/keyring/ssh
+TERM=xterm-256color
+USER=ros2
+USERNAME=ros2
+VTE_VERSION=6003
+WINDOWPATH=2
+XAUTHORITY=/run/user/1000/gdm/Xauthority
+XDG_CONFIG_DIRS=/etc/xdg/xdg-ubuntu:/etc/xdg
+XDG_CURRENT_DESKTOP=ubuntu:GNOME
+XDG_DATA_DIRS=/usr/share/ubuntu:/usr/local/share/:/usr/share/:/var/lib/snapd/desktop
+XDG_MENU_PREFIX=gnome-
+XDG_RUNTIME_DIR=/run/user/1000
+XDG_SESSION_CLASS=user
+XDG_SESSION_DESKTOP=ubuntu
+XDG_SESSION_TYPE=x11
+XMODIFIERS=@im=ibus
+_=/usr/bin/colcon

build/aruco_detector/install.log

@@ -0,0 +1,18 @@
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector/transformations.py
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector/yaml_handling.py
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector/aruco_detector_node.py
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector/__init__.py
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector/__pycache__/transformations.cpython-38.pyc
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector/__pycache__/yaml_handling.cpython-38.pyc
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector/__pycache__/aruco_detector_node.cpython-38.pyc
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector/__pycache__/__init__.cpython-38.pyc
+/home/ros2/dev2_ws/install/aruco_detector/share/ament_index/resource_index/packages/aruco_detector
+/home/ros2/dev2_ws/install/aruco_detector/share/aruco_detector/package.xml
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector-0.0.0-py3.8.egg-info/requires.txt
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector-0.0.0-py3.8.egg-info/top_level.txt
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector-0.0.0-py3.8.egg-info/dependency_links.txt
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector-0.0.0-py3.8.egg-info/PKG-INFO
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector-0.0.0-py3.8.egg-info/entry_points.txt
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector-0.0.0-py3.8.egg-info/zip-safe
+/home/ros2/dev2_ws/install/aruco_detector/lib/python3.8/site-packages/aruco_detector-0.0.0-py3.8.egg-info/SOURCES.txt
+/home/ros2/dev2_ws/install/aruco_detector/lib/aruco_detector/aruco_detector_node