bruecknerja86834/IoT_Furniture_Localization
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

aruco_detector: kommentiert

Browse Source
This commit is contained in:
Janik Brueckner 2022-02-27 20:39:54 +01:00
parent d24b67cb3d
commit 8d1278c7a1
2 changed files with 54 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -44,7 +44,7 @@ An overview about the running topics is illustrated in the next picture.
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 .
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`

77
src/aruco_detector/aruco_detector/aruco_detector_node.py
View File

@ -1,3 +1,40 @@
#################################################################################
# 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
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
@ -17,31 +54,11 @@ from aruco_interfaces.msg import ArucoMarkerPose
########################################################
# 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):
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")
@ -78,13 +95,17 @@ class ArucoDetectorNode(rclpy.node.Node):
if camera_info_msg is None:
self.get_logger().warn("No valid camera info inside the yaml_file!")
return
# read and store camera parameters
self.intrinsic_mat = np.reshape(np.array(camera_info_msg.k), (3, 3))
self.distortion = np.array(camera_info_msg.d)
# get the dictionary / parameters
self.aruco_dictionary = cv2.aruco.Dictionary_get(dictionary_id)
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()
def image_callback(self, img_msg):
@ -123,27 +144,32 @@ class ArucoDetectorNode(rclpy.node.Node):
# iterate through all founded marker_ids and compute their poses
for i, marker_id in enumerate(marker_ids):
# store the calulated current position (translation vector)
pose = Pose()
pose.position.x = tvecs[i][0][0]
pose.position.y = tvecs[i][0][1]
pose.position.z = tvecs[i][0][2]
# create the rotation matrix
rot_matrix = np.eye(4)
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)
# 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.y = 0.0
pose.orientation.z = math.sin((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.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)
# Draw the Axis into the output picture
@ -157,6 +183,7 @@ class ArucoDetectorNode(rclpy.node.Node):
# Draw a rectangle around the detected markers to highlight them.
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')
# Publish the finished picture with the highlighted Markers and Axis