From 0fde086b24ee86f7220f67edff9de9bbf685a681 Mon Sep 17 00:00:00 2001 From: TheNoobInventor Date: Mon, 29 Apr 2024 21:10:53 +0100 Subject: [PATCH] Update aruco trajectory visualizer node --- lidarbot_aruco/README.md | 20 ++ .../config/chessboard_calibration.yaml | 4 + lidarbot_aruco/config/params_1.yaml | 23 ++ lidarbot_aruco/config/params_2.yaml | 24 ++ .../aruco_trajectory_visualizer.py | 247 ++++++++++++++++++ .../lidarbot_aruco/detect_aruco_marker.py | 1 + .../lidarbot_aruco/generate_aruco_marker.py | 4 +- lidarbot_aruco/setup.py | 2 +- lidarbot_aruco/tags/DICT_4X4_50_id1.png | Bin 0 -> 750 bytes 9 files changed, 322 insertions(+), 3 deletions(-) mode change 100644 => 100755 lidarbot_aruco/config/chessboard_calibration.yaml create mode 100755 lidarbot_aruco/config/params_1.yaml create mode 100755 lidarbot_aruco/config/params_2.yaml create mode 100644 lidarbot_aruco/tags/DICT_4X4_50_id1.png diff --git a/lidarbot_aruco/README.md b/lidarbot_aruco/README.md index 8cb3ad1..bef5d0d 100644 --- a/lidarbot_aruco/README.md +++ b/lidarbot_aruco/README.md @@ -39,3 +39,23 @@ Running the previous script opens a window with the generated ArUco tag displaye To close the window, press the **q** key on the keyboard on the opened window. +## Camera calibration + +The Logitech webcam C270 HD is used in this project and needs to be calibrated. + +First install the [ROS usb camera driver](https://index.ros.org/r/usb_cam/#humble) package: + +```bash +sudo apt install ros-humble-usb-cam +``` + +Camera calibration was done following the steps outlined this [guide](https://navigation.ros.org/tutorials/docs/camera_calibration.html). + +TODO: Note about 'dt' field in calibration file. Reference Addison's calibration guide. + +Execute the command below to run the usb-cam driver node: + +```bash +ros2 run usb_cam usb_cam_node_exe --ros-args --params-file ~/dev_ws/src/lidarbot_aruco/config/params_1.yaml +``` + diff --git a/lidarbot_aruco/config/chessboard_calibration.yaml b/lidarbot_aruco/config/chessboard_calibration.yaml old mode 100644 new mode 100755 index 8d62219..ed772c2 --- a/lidarbot_aruco/config/chessboard_calibration.yaml +++ b/lidarbot_aruco/config/chessboard_calibration.yaml @@ -1,9 +1,12 @@ +%YAML:1.0 +--- image_width: 640 image_height: 480 camera_name: narrow_stereo camera_matrix: rows: 3 cols: 3 + dt: d data: [842.57825, 0. , 304.89032, 0. , 841.64125, 224.5223 , 0. , 0. , 1. ] @@ -11,6 +14,7 @@ distortion_model: plumb_bob distortion_coefficients: rows: 1 cols: 5 + dt: d data: [0.002363, 0.562980, -0.004719, -0.008675, 0.000000] rectification_matrix: rows: 3 diff --git a/lidarbot_aruco/config/params_1.yaml b/lidarbot_aruco/config/params_1.yaml new file mode 100755 index 0000000..9f24593 --- /dev/null +++ b/lidarbot_aruco/config/params_1.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + video_device: "/dev/video0" + framerate: 30.0 + io_method: "mmap" + frame_id: "camera" + pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats + av_device_format: "YUV422P" + image_width: 640 + image_height: 480 + camera_name: "test_camera" + camera_info_url: "package://usb_cam/config/camera_info.yaml" + brightness: -1 + contrast: -1 + saturation: -1 + sharpness: -1 + gain: -1 + auto_white_balance: true + white_balance: 4000 + autoexposure: true + exposure: 100 + autofocus: false + focus: -1 diff --git a/lidarbot_aruco/config/params_2.yaml b/lidarbot_aruco/config/params_2.yaml new file mode 100755 index 0000000..b3c6cb8 --- /dev/null +++ b/lidarbot_aruco/config/params_2.yaml @@ -0,0 +1,24 @@ +/**: + ros__parameters: + video_device: "/dev/video2" + framerate: 15.0 + io_method: "mmap" + frame_id: "camera2" + pixel_format: "mjpeg2rgb" + av_device_format: "YUV422P" + image_width: 640 + image_height: 480 + camera_name: "test_camera2" + # reusing same camera intrinsics only for demo, should really be unique for camera2" + camera_info_url: "package://usb_cam/config/camera_info.yaml" + brightness: -1 + contrast: -1 + saturation: -1 + sharpness: -1 + gain: -1 + auto_white_balance: true + white_balance: 4000 + autoexposure: true + exposure: 100 + autofocus: false + focus: -1 \ No newline at end of file diff --git a/lidarbot_aruco/lidarbot_aruco/aruco_trajectory_visualizer.py b/lidarbot_aruco/lidarbot_aruco/aruco_trajectory_visualizer.py index e69de29..ae01c3c 100755 --- a/lidarbot_aruco/lidarbot_aruco/aruco_trajectory_visualizer.py +++ b/lidarbot_aruco/lidarbot_aruco/aruco_trajectory_visualizer.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python3 + +# This node is used to visualize the trajectory of lidarbot by tracking the ArUco marker +# on it + +# Adapted from: https://automaticaddison.com/estimate-aruco-marker-pose-using-opencv-gazebo-and-ros-2/ + +# Import Python libraries +import cv2 +import numpy as np +import argparse +from scipy.spatial.transform import Rotation as R + +# Import ROS2 libraries +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data # Uses Best Effort reliability for camera +from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images +from geometry_msgs.msg import TransformStamped # Handles TransformStamped message +from sensor_msgs.msg import Image # Image is the message type +from tf2_ros import TransformBroadcaster + +# Construct argument parser and parse the arguments +ap = argparse.ArgumentParser() +ap.add_argument( + "-t", "--type", type=str, default="DICT_4X4_50", help="type of ArUco tag to detect" +) + +args = vars(ap.parse_args()) + +# The different ArUco dictionaries built into the OpenCV library +ARUCO_DICT = { + "DICT_4X4_50": cv2.aruco.DICT_4X4_50, + "DICT_4X4_100": cv2.aruco.DICT_4X4_100, + "DICT_4X4_250": cv2.aruco.DICT_4X4_250, + "DICT_4X4_1000": cv2.aruco.DICT_4X4_1000, + "DICT_5X5_50": cv2.aruco.DICT_5X5_50, + "DICT_5X5_100": cv2.aruco.DICT_5X5_100, + "DICT_5X5_250": cv2.aruco.DICT_5X5_250, + "DICT_5X5_1000": cv2.aruco.DICT_5X5_1000, + "DICT_6X6_50": cv2.aruco.DICT_6X6_50, + "DICT_6X6_100": cv2.aruco.DICT_6X6_100, + "DICT_6X6_250": cv2.aruco.DICT_6X6_250, + "DICT_6X6_1000": cv2.aruco.DICT_6X6_1000, + "DICT_7X7_50": cv2.aruco.DICT_7X7_50, + "DICT_7X7_100": cv2.aruco.DICT_7X7_100, + "DICT_7X7_250": cv2.aruco.DICT_7X7_250, + "DICT_7X7_1000": cv2.aruco.DICT_7X7_1000, + "DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL, +} + + +class ArucoNode(Node): + def __init__(self): + super().__init__("aruco_node") + + # Declare parameters + self.declare_parameter("aruco_dictionary_name", "DICT_4X4_50") + self.declare_parameter("aruco_marker_side_length", 0.063) + self.declare_parameter( + "camera_calibration_parameters_filename", + "/home/noobinventor/lidarbot_ws/src/lidarbot_aruco/config/chessboard_calibration.yaml", + ) + self.declare_parameter("image_topic", "/image_raw") + self.declare_parameter("aruco_marker_name", "aruco_marker_frame") + + # Read parameters + aruco_dictionary_name = ( + self.get_parameter("aruco_dictionary_name") + .get_parameter_value() + .string_value + ) + self.aruco_marker_side_length = ( + self.get_parameter("aruco_marker_side_length") + .get_parameter_value() + .double_value + ) + self.camera_calibration_parameters_filename = ( + self.get_parameter("camera_calibration_parameters_filename") + .get_parameter_value() + .string_value + ) + image_topic = ( + self.get_parameter("image_topic").get_parameter_value().string_value + ) + self.aruco_marker_name = ( + self.get_parameter("aruco_marker_name").get_parameter_value().string_value + ) + + # Check that we have a valid ArUco marker + if ARUCO_DICT.get(aruco_dictionary_name, None) is None: + self.get_logger().info( + "[INFO] ArUCo tag of '{}' is not supported".format(args["type"]) + ) + + # Load the camera parameters from the saved file + cv_file = cv2.FileStorage( + self.camera_calibration_parameters_filename, cv2.FILE_STORAGE_READ + ) + self.camera_matrix = cv_file.getNode("camera_matrix").mat() + self.dist_coeffs = cv_file.getNode("distortion_coefficients").mat() + cv_file.release() + + # Load the ArUco dictionary + self.get_logger().info( + "[INFO] detecting '{}' markers...".format(aruco_dictionary_name) + ) + self.arucoDict = cv2.aruco.getPredefinedDictionary(ARUCO_DICT[args["type"]]) + self.arucoParams = cv2.aruco.DetectorParameters() + + # Create the subscriber. This subscriber will receive an Image + # from the image_topic + self.subscription = self.create_subscription( + Image, + image_topic, + self.listener_callback, + qos_profile=qos_profile_sensor_data, + ) + self.subscription # prevent unused variable warning + + # Initialize the transform broadcaster + self.tfbroadcaster = TransformBroadcaster(self) + + # Used to convert between ROS and OpenCV images + self.bridge = CvBridge() + + # Callback function + def listener_callback(self, data): + # Display the message on the console + self.get_logger().info("Receiving video frame") + + # Convert ROS Image message to OpenCV image + current_frame = self.bridge.imgmsg_to_cv2(data, desired_encoding="bgr8") + + # Detect ArUco markers in the video frame + (corners, marker_ids, rejected) = cv2.aruco.detectMarkers( + current_frame, + self.arucoDict, + parameters=self.arucoParams, + ) + + # Check that at least one ArUco marker was detected + if marker_ids is not None: + + # Draw a square around detected markers in the video frame + cv2.aruco.drawDetectedMarkers(current_frame, corners, marker_ids) + + # Get the rotation and translation vectors + rvecs, tvecs, obj_points = cv2.aruco.estimatePoseSingleMarkers( + corners, + self.aruco_marker_side_length, + self.camera_matrix, + self.dist_coeffs, + ) + + # The pose of the marker is with respect to the camera lens frame. + # Imagine you are looking through the camera viewfinder, + # the camera lens frame's: + # x-axis points to the right + # y-axis points straight down towards your toes + # z-axis points straight ahead away from your eye, out of the camera + for i, marker_id in enumerate(marker_ids): + + # Create the coordinate transform + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = "webcam_frame" + t.child_frame_id = self.aruco_marker_name + + # Store the translation (i.e. position) information + t.transform.translation.x = tvecs[i][0][0] + t.transform.translation.y = tvecs[i][0][1] + t.transform.translation.z = tvecs[i][0][2] + + # Store the rotation information + rotation_matrix = np.eye(4) + rotation_matrix[0:3, 0:3] = cv2.Rodrigues(np.array(rvecs[i][0]))[0] + r = R.from_matrix(rotation_matrix[0:3, 0:3]) + quat = r.as_quat() + + # Quaternion format + t.transform.rotation.x = quat[0] + t.transform.rotation.y = quat[1] + t.transform.rotation.z = quat[2] + t.transform.rotation.w = quat[3] + + # Send the transform + self.tfbroadcaster.sendTransform(t) + + # Draw the axes on the marker + cv2.drawFrameAxes( + current_frame, + self.camera_matrix, + self.dist_coeffs, + rvecs[i], + tvecs[i], + 0.05, + ) + + # Draw circle at the center of ArUco tag + # x_sum = ( + # corners[0][0][0][0] + # + corners[0][0][1][0] + # + corners[0][0][2][0] + # + corners[0][0][3][0] + # ) + # y_sum = ( + # corners[0][0][0][1] + # + corners[0][0][1][1] + # + corners[0][0][2][1] + # + corners[0][0][3][1] + # ) + # + # x_centerPixel = x_sum * 0.25 + # y_centerPixel = y_sum * 0.25 + # + # cv2.circle( + # current_frame, (x_centerPixel, y_centerPixel), 4, (0, 0, 255), -1 + # ) + + # Display image for testing + cv2.imshow("camera", current_frame) + cv2.waitKey(1) + + +def main(args=None): + + # Initialize the rclpy library + rclpy.init(args=args) + + # Create the node + aruco_node = ArucoNode() + + # Spin the node so the callback function is called + rclpy.spin(aruco_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + aruco_node.destroy_node() + + # Shutdown the ROS client library for Python + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/lidarbot_aruco/lidarbot_aruco/detect_aruco_marker.py b/lidarbot_aruco/lidarbot_aruco/detect_aruco_marker.py index cf4e753..0b5c6cc 100755 --- a/lidarbot_aruco/lidarbot_aruco/detect_aruco_marker.py +++ b/lidarbot_aruco/lidarbot_aruco/detect_aruco_marker.py @@ -7,6 +7,7 @@ import argparse import cv2 +import sys # Construct argument parser and parse the arguments ap = argparse.ArgumentParser() diff --git a/lidarbot_aruco/lidarbot_aruco/generate_aruco_marker.py b/lidarbot_aruco/lidarbot_aruco/generate_aruco_marker.py index a421cae..f4ecce6 100755 --- a/lidarbot_aruco/lidarbot_aruco/generate_aruco_marker.py +++ b/lidarbot_aruco/lidarbot_aruco/generate_aruco_marker.py @@ -71,8 +71,8 @@ def main(): args["type"], args["id"] ) ) - tag = np.zeros((250, 250, 1), dtype="uint8") - cv2.aruco.generateImageMarker(arucoDict, args["id"], 250, tag, 1) + tag = np.zeros((180, 180, 1), dtype="uint8") + cv2.aruco.generateImageMarker(arucoDict, args["id"], 180, tag, 1) # Write the generated ArUCo tag to disk and then display it to our # screen diff --git a/lidarbot_aruco/setup.py b/lidarbot_aruco/setup.py index 81631c4..714fa17 100644 --- a/lidarbot_aruco/setup.py +++ b/lidarbot_aruco/setup.py @@ -25,7 +25,7 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "aruco_trajectory_visualizer_node = lidarbot_aruco.aruco_trajectory_visualizer.main" + "aruco_trajectory_visualizer_node = lidarbot_aruco.aruco_trajectory_visualizer:main" ], }, ) diff --git a/lidarbot_aruco/tags/DICT_4X4_50_id1.png b/lidarbot_aruco/tags/DICT_4X4_50_id1.png new file mode 100644 index 0000000000000000000000000000000000000000..a87e4fa762e00332a028752480d2ec1f635e34c5 GIT binary patch literal 750 zcmeAS@N?(olHy`uVBq!ia0vp^TR@ltNH9n&i+snxz_iuV#WAFU@#;ZC!2=EgM+|1$ zzu;vSzn|hasriY5r3W7!D3gDRu9tqai+Dbk4^ru^_n1g$fyF0w2QKlDa=q89i-GRw+9U1SwIuM#T@9eal-|2dRLT^oH187e99&vbMJm-ZMKl+mu=ZT+ zA?bS0Q_A&}#wWi^8bLsR0R3XrwP&(P*Ob5~bC(1z=}~$Q^vmGULPj!Mz^`C2ZEMez RqA*ZG_H^}gS?83{1OS>eF@XR8 literal 0 HcmV?d00001