diff --git a/README.md b/README.md index b77274b..9db10b3 100644 --- a/README.md +++ b/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--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. -## Outputs +![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 . + +- 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") \ No newline at end of file diff --git a/src/aruco_detector/aruco_detector/aruco_detector_node.py b/src/aruco_detector/aruco_detector/aruco_detector_node.py index 2534486..a933702 100644 --- a/src/aruco_detector/aruco_detector/aruco_detector_node.py +++ b/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)