Browse Source

aruco_detector: kommentiert

master
Janik Brueckner 2 years ago
parent
commit
8d1278c7a1
2 changed files with 54 additions and 27 deletions
  1. 1
    1
      README.md
  2. 53
    26
      src/aruco_detector/aruco_detector/aruco_detector_node.py

+ 1
- 1
README.md View File

int64[] marker_ids int64[] marker_ids
geometry_msgs/Pose[] poses 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 .
The array `marker_ids` contains all detected ids 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` - topic: `/aruco/aruco_output_images/compressed`



+ 53
- 26
src/aruco_detector/aruco_detector/aruco_detector_node.py View File

#################################################################################
# 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 import rclpy
from rclpy.node import Node from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data from rclpy.qos import qos_profile_sensor_data






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


class ArucoDetectorNode(rclpy.node.Node): class ArucoDetectorNode(rclpy.node.Node):
def __init__(self): def __init__(self):
super().__init__('aruco_detector_node') super().__init__('aruco_detector_node')


# Declare parameters # Declare parameters
self.declare_parameter("marker_size", .1) self.declare_parameter("marker_size", .1)
self.declare_parameter("aruco_dictionary_id", "DICT_6X6_100") self.declare_parameter("aruco_dictionary_id", "DICT_6X6_100")
camera_info_msg = yaml_handling.yaml_to_CameraInfo(url_yaml_file) camera_info_msg = yaml_handling.yaml_to_CameraInfo(url_yaml_file)
if camera_info_msg is None: if camera_info_msg is None:
self.get_logger().warn("No valid camera info inside the yaml_file!") self.get_logger().warn("No valid camera info inside the yaml_file!")
return

return
# read and store camera parameters
self.intrinsic_mat = np.reshape(np.array(camera_info_msg.k), (3, 3)) self.intrinsic_mat = np.reshape(np.array(camera_info_msg.k), (3, 3))
self.distortion = np.array(camera_info_msg.d) self.distortion = np.array(camera_info_msg.d)


# get the dictionary / parameters
self.aruco_dictionary = cv2.aruco.Dictionary_get(dictionary_id) self.aruco_dictionary = cv2.aruco.Dictionary_get(dictionary_id)
self.aruco_parameters = cv2.aruco.DetectorParameters_create() 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() self.bridge = CvBridge()


def image_callback(self, img_msg): def image_callback(self, img_msg):


# iterate through all founded marker_ids and compute their poses # iterate through all founded marker_ids and compute their poses
for i, marker_id in enumerate(marker_ids): for i, marker_id in enumerate(marker_ids):
# store the calulated current position (translation vector)
pose = Pose() pose = Pose()
pose.position.x = tvecs[i][0][0] pose.position.x = tvecs[i][0][0]
pose.position.y = tvecs[i][0][1] pose.position.y = tvecs[i][0][1]
pose.position.z = tvecs[i][0][2] pose.position.z = tvecs[i][0][2]


# create the rotation matrix
rot_matrix = np.eye(4) rot_matrix = np.eye(4)
rot_matrix[0:3, 0:3] = cv2.Rodrigues(np.array(rvecs[i][0]))[0] rot_matrix[0:3, 0:3] = cv2.Rodrigues(np.array(rvecs[i][0]))[0]
angle = math.acos(rot_matrix[0][0])*(180/math.pi)

# calculate the current angle
angle_psi, angle_theta, angle_phi = mat2euler(rot_matrix) 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.x = 0.0
pose.orientation.y = 0.0 pose.orientation.y = 0.0
pose.orientation.z = math.sin((angle_phi) / 2) pose.orientation.z = math.sin((angle_phi) / 2)
pose.orientation.w = math.cos((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.poses.append(pose)
pose_array.marker_ids.append(marker_id[0]) pose_array.marker_ids.append(marker_id[0])


# publish the pose array when all markers are detected
# publish the pose list when all markers are detected
self.publisher_marker_poses.publish(pose_array) self.publisher_marker_poses.publish(pose_array)


# Draw the Axis into the output picture # Draw the Axis into the output picture


# Draw a rectangle around the detected markers to highlight them. # Draw a rectangle around the detected markers to highlight them.
output_img = cv2.aruco.drawDetectedMarkers(cv_image, corners, marker_ids) 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') output_img_compressed = self.bridge.cv2_to_compressed_imgmsg(output_img, dst_format='jpg')


# Publish the finished picture with the highlighted Markers and Axis # Publish the finished picture with the highlighted Markers and Axis

Loading…
Cancel
Save