Readme aktualisiert + Kommentar in aruco_detector_node

README.md

@@ -16,9 +16,10 @@ This project was created as an project thesis (MSY-Master).
 
     Usb_cam is a frequently used ros driver for V4L USB cameras. A detailed description can be found here: http://wiki.ros.org/usb_cam
 
-
         sudo apt get install ros-<ros2-distro>-usb-cam
 
+    Important: It's necessary to work with compressed images. Therefor you can use the image_transport library. A detailed description can be found here: http://wiki.ros.org/image_transport
+
     
 
 ## Running the code
@@ -30,8 +31,23 @@ For an easy start there is already created a launch file. All required parameter
         cd launch/
         ros2 launch aruco_launch.py
 
-    ![Bild 1](./Images/Topics.jpg "ArUco Marker attached on robot")
+An overview about the running topics is illustrated in the next picture. 
+
+![Bild 1](./Images/Topics.jpg "started topics")
+
+## Output
+
+- topic: `/aruco/aruco_poses`
+
+    The aruco_detector_node publishes a custom ros2 message. 
+
+            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 . 
 
-## Outputs
+- 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.
 
+    ![Bild 1](./Images/Output_pic.jpg "started topics")

src/aruco_detector/aruco_detector/aruco_detector_node.py

@@ -6,6 +6,7 @@ from cv_bridge import CvBridge
 import cv2
 
 import numpy as np
+import math
 from transforms3d.euler import mat2euler
 
 from aruco_detector import yaml_handling 
@@ -130,16 +131,10 @@ class ArucoDetectorNode(rclpy.node.Node):
                 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)
 
-                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)))
-
                 pose.orientation.x = 0.0 
                 pose.orientation.y = 0.0 
                 pose.orientation.z = math.sin((angle_phi) / 2)