Browse Source

Readme aktualisiert + Kommentar in aruco_detector_node

Janik Brueckner 2 years ago
2 changed files with 20 additions and 9 deletions
  1. 19
  2. 1

+ 19
- 3 View File

@@ -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:

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:

## 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

![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")

+ 1
- 6
src/aruco_detector/aruco_detector/ View File

@@ -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)