bruecknerja86834
/
IoT_Furniture_Localization
Watch 1
Star 0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

aruco_detector: kommentiert

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

+ 1
- 1
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`


+ 53
- 26
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")
@@ -77,14 +94,18 @@ class ArucoDetectorNode(rclpy.node.Node):
camera_info_msg = yaml_handling.yaml_to_CameraInfo(url_yaml_file)
if camera_info_msg is None:
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.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

Write Preview
Loading…
Cancel
Save