From d487160e38c418bce3de9ffcf99ebb567ba0dc86 Mon Sep 17 00:00:00 2001 From: Jose Manuel Diez Date: Mon, 8 Feb 2021 14:28:48 +0100 Subject: [PATCH 1/3] Fix relative imports, replace `print x` with `print(x)` --- aruco_analyzer/src/aruco_analyzer/__init__.py | 2 +- aruco_analyzer/src/aruco_analyzer/aruco_detection.py | 8 ++++---- .../aruco_analyzer/helper_classes/image_distributor.py | 6 +++--- .../src/aruco_analyzer/ros_wrapper/__init__.py | 10 +++++----- .../ros_wrapper/aruco_detection_ros_wrapper.py | 10 +++++----- .../ros_wrapper/detection_image_publisher.py | 4 ++-- 6 files changed, 20 insertions(+), 20 deletions(-) diff --git a/aruco_analyzer/src/aruco_analyzer/__init__.py b/aruco_analyzer/src/aruco_analyzer/__init__.py index fb58721..901caed 100755 --- a/aruco_analyzer/src/aruco_analyzer/__init__.py +++ b/aruco_analyzer/src/aruco_analyzer/__init__.py @@ -1 +1 @@ -from aruco_detection import ARMarkerDetector \ No newline at end of file +from .aruco_detection import ARMarkerDetector diff --git a/aruco_analyzer/src/aruco_analyzer/aruco_detection.py b/aruco_analyzer/src/aruco_analyzer/aruco_detection.py index 8b05258..7be08eb 100644 --- a/aruco_analyzer/src/aruco_analyzer/aruco_detection.py +++ b/aruco_analyzer/src/aruco_analyzer/aruco_detection.py @@ -9,10 +9,10 @@ except ImportError: from yaml import Loader -from helper_classes.aruco_detector import ArucoDetector -from helper_classes.analyzer import Analyzer -from helper_classes.image_distributor import ImageDistributor -from helper_classes.config import Config +from .helper_classes.aruco_detector import ArucoDetector +from .helper_classes.analyzer import Analyzer +from .helper_classes.image_distributor import ImageDistributor +from .helper_classes.config import Config class ARMarkerDetector(object): diff --git a/aruco_analyzer/src/aruco_analyzer/helper_classes/image_distributor.py b/aruco_analyzer/src/aruco_analyzer/helper_classes/image_distributor.py index 7b9d9be..693134b 100644 --- a/aruco_analyzer/src/aruco_analyzer/helper_classes/image_distributor.py +++ b/aruco_analyzer/src/aruco_analyzer/helper_classes/image_distributor.py @@ -1,12 +1,12 @@ #!/usr/bin/env python import logging from threading import Thread, Lock, Condition, Event -try: +try: from queue import Queue except ImportError: from Queue import Queue -from config import Config - +from .config import Config + class ImageDistributor(object): logger = logging.getLogger(__name__) diff --git a/aruco_analyzer/src/aruco_analyzer/ros_wrapper/__init__.py b/aruco_analyzer/src/aruco_analyzer/ros_wrapper/__init__.py index 8c3d346..a86ffd5 100644 --- a/aruco_analyzer/src/aruco_analyzer/ros_wrapper/__init__.py +++ b/aruco_analyzer/src/aruco_analyzer/ros_wrapper/__init__.py @@ -1,5 +1,5 @@ -from aruco_detection_ros_wrapper import * -from detection_image_publisher import * -from cpumem import * -from ros_image_miner import * -from tf_broadcaster import * \ No newline at end of file +from .aruco_detection_ros_wrapper import * +from .detection_image_publisher import * +from .cpumem import * +from .ros_image_miner import * +from .tf_broadcaster import * diff --git a/aruco_analyzer/src/aruco_analyzer/ros_wrapper/aruco_detection_ros_wrapper.py b/aruco_analyzer/src/aruco_analyzer/ros_wrapper/aruco_detection_ros_wrapper.py index e2ad00d..2b5dcd9 100644 --- a/aruco_analyzer/src/aruco_analyzer/ros_wrapper/aruco_detection_ros_wrapper.py +++ b/aruco_analyzer/src/aruco_analyzer/ros_wrapper/aruco_detection_ros_wrapper.py @@ -1,11 +1,11 @@ #!/usr/bin/env python import rospy from threading import Thread -from aruco_analyzer import ARMarkerDetector -from detection_image_publisher import DetectionImagePublisher -from ros_image_miner import ROSImageMiner -from cpumem import CpuMemMonitor -from tf_broadcaster import TFBroadCaster +from ..aruco_detection import ARMarkerDetector +from .detection_image_publisher import DetectionImagePublisher +from .ros_image_miner import ROSImageMiner +from .cpumem import CpuMemMonitor +from .tf_broadcaster import TFBroadCaster class ARMarkerDetectorWrapper (object): def __init__(self): diff --git a/aruco_analyzer/src/aruco_analyzer/ros_wrapper/detection_image_publisher.py b/aruco_analyzer/src/aruco_analyzer/ros_wrapper/detection_image_publisher.py index 56568e0..53b4174 100644 --- a/aruco_analyzer/src/aruco_analyzer/ros_wrapper/detection_image_publisher.py +++ b/aruco_analyzer/src/aruco_analyzer/ros_wrapper/detection_image_publisher.py @@ -26,5 +26,5 @@ def run(self): self.image_publishers[camera].publish( self.bridge.cv2_to_compressed_imgmsg(self.detection_images_qdic[camera].get())) except CvBridgeError as e: - print e - self._detection_image_available.clear() \ No newline at end of file + print(e) + self._detection_image_available.clear() From 8b99edeaa7a1d2541a52c8b03866320fbcaafc5e Mon Sep 17 00:00:00 2001 From: Jose Manuel Diez Date: Mon, 8 Feb 2021 14:30:27 +0100 Subject: [PATCH 2/3] Fix camera namespace in launch script. --- aruco_analyzer/launch/aruco_analyzer_node.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aruco_analyzer/launch/aruco_analyzer_node.launch b/aruco_analyzer/launch/aruco_analyzer_node.launch index 02533e8..7be1189 100644 --- a/aruco_analyzer/launch/aruco_analyzer_node.launch +++ b/aruco_analyzer/launch/aruco_analyzer_node.launch @@ -1,7 +1,7 @@ - - + + From 897593c11fadb772ff166bf411fd48cd51f19511 Mon Sep 17 00:00:00 2001 From: Jose Manuel Diez Date: Mon, 8 Feb 2021 14:31:57 +0100 Subject: [PATCH 3/3] Pass `rvec` and `tvec` to `cv2.aruco.estimate*` fns. --- .../aruco_analyzer/helper_classes/aruco_detector.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/aruco_analyzer/src/aruco_analyzer/helper_classes/aruco_detector.py b/aruco_analyzer/src/aruco_analyzer/helper_classes/aruco_detector.py index 289b343..c9ddcbf 100644 --- a/aruco_analyzer/src/aruco_analyzer/helper_classes/aruco_detector.py +++ b/aruco_analyzer/src/aruco_analyzer/helper_classes/aruco_detector.py @@ -67,16 +67,15 @@ def detect_board(self, camera_image, board): if self.config.draw_detected_markers: cv2.aruco.drawDetectedMarkers(camera_image.image, corners) + rvec = np.zeros(3) + tvec = np.zeros(3) retval, rvec, tvec = cv2.aruco.estimatePoseBoard( - corners, ids, board.board, camera_image.camera.camera_matrix, camera_image.camera.distortion_coefficients + corners, ids, board.board, camera_image.camera.camera_matrix, camera_image.camera.distortion_coefficients, rvec, tvec ) if retval is 0: return None - rvec = rvec.reshape(-1) - tvec = tvec.reshape(-1) - if self.config.draw_axis: cv2.aruco.drawAxis(camera_image.image, camera_image.camera.camera_matrix, camera_image.camera.distortion_coefficients, rvec, tvec, board.marker_length) @@ -84,7 +83,7 @@ def detect_board(self, camera_image, board): dist = np.linalg.norm(tvec) strg = 'ID {} distance: {:.3f}m'.format(board.id, dist) - strg2 = 'x: {:.3f} y: {:.3f} z: {:.3f}'.format(*tvec) + strg2 = 'x: {:.3f} y: {:.3f} z: {:.3f}'.format(tvec[0], tvec[1], tvec[2]) strg3 = 'r: {:.3f} p: {:.3f} y: {:.3f}'.format(*map(math.degrees, rvec)) cv2.putText(camera_image.image, strg, (0, text_place), self.font, @@ -95,7 +94,7 @@ def detect_board(self, camera_image, board): text_place += 30 cv2.putText(camera_image.image, strg3, (0, text_place), self.font, 1, (0, 0, 255), 2, cv2.LINE_AA) - + output = DetectionOutput() output.pack(camera_image, np.array([board.first_marker]), [rvec], [tvec], [board.type]) return output