Various maintenance fixes

aruco_analyzer/launch/aruco_analyzer_node.launch

@@ -1,7 +1,7 @@
 <launch>
 	<node name="aruco_analyzer" pkg="aruco_analyzer" type="aruco_detection_node" output="screen" respawn="false" clear_params="true"> 
 		<rosparam command="load" file="$(find aruco_analyzer)/config/config.yaml" param="config"/>
-		<param name="transport_hint" value="/color/image_raw" />
-		<param name="camera_ns" value="/sensors" />
+		<param name="transport_hint" value="/image_raw" />
+		<param name="camera_ns" value="/cameras" />
 	</node>
 </launch>

aruco_analyzer/src/aruco_analyzer/__init__.py

@@ -1 +1 @@
-from aruco_detection import ARMarkerDetector
+from .aruco_detection import ARMarkerDetector

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):
 

aruco_analyzer/src/aruco_analyzer/helper_classes/aruco_detector.py

@@ -67,24 +67,23 @@ 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)
 
         if self.config.print_in_image:
             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

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__)

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 *
+from .aruco_detection_ros_wrapper import *
+from .detection_image_publisher import *
+from .cpumem import *
+from .ros_image_miner import *
+from .tf_broadcaster import *

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):

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()
+                        print(e)
+            self._detection_image_available.clear()