diff --git a/src/add_qr.py b/src/add_qr.py
index 095a8a3e1ae1db770956f3dd37a5d98857745619..86beb0741286e4f945704495500f81ddfa6f5d54 100755
--- a/src/add_qr.py
+++ b/src/add_qr.py
@@ -56,10 +56,10 @@ def json_to_transform(jobj):
res.translation.x = jobj["tra"]["x"]
res.translation.y = jobj["tra"]["y"]
res.translation.z = jobj["tra"]["z"]
- res.rotation.x = jobj["rot"]["x"]
- res.rotation.y = jobj["rot"]["y"]
- res.rotation.z = jobj["rot"]["z"]
- res.rotation.w = jobj["rot"]["w"]
+ res.rotation.x = jobj["rot"]["x"]
+ res.rotation.y = jobj["rot"]["y"]
+ res.rotation.z = jobj["rot"]["z"]
+ res.rotation.w = jobj["rot"]["w"]
return res
def header_to_json(data):
@@ -82,7 +82,7 @@ def image_callback(data):
global fifo, remembered_points, have_points
try:
# print("image_callback_main")
- if options.compressed:
+ if options.compressed:
img = bridge.compressed_imgmsg_to_cv2(data, desired_encoding='bgr8')
else:
img = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
@@ -94,7 +94,7 @@ def image_callback(data):
look_up_time = rospy.Time(0)
# look_up_time = data.header.stamp
-
+
# print("TRANS-ROT:", trans, rot)
if options.qr_transform:
@@ -117,14 +117,14 @@ def image_callback(data):
jobj = transform_to_json(transform)
# print("DUMPS:", json.dumps(jobj))
-
+
scaledimg = cv2.resize(img, (1024, 600))
if options.qr_transform or options.qr_header or options.qr_time:
# print("SCALEDIMG:", scaledimg.shape, scaledimg.dtype)
frame_pil = PilImage.fromarray(scaledimg)
-
+
qr = qrcode.QRCode(box_size=2)
if options.qr_time:
timestamp = time.time()
@@ -136,48 +136,50 @@ def image_callback(data):
pos = (frame_pil.size[0] - img_qr.size[0] - 1, 1) #better result when 1 pixel in, thus -1 and +1
# print("POS QR:", pos)
-
+
frame_pil.paste(img_qr, pos)
final_image = np.asarray(frame_pil)
else:
final_image = scaledimg
# print("FINALIMAGE:", final_image.shape, final_image.dtype)
-
+
#gray = numpy.array(img_qr).astype(numpy.float32)
#print("GRAY:", gray.shape, gray.dtype)
#cv_img_qr = cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB)
- #print("CV_IMG_QR:", cv_img_qr.shape, cv_img_qr.dtype)
+ #print("CV_IMG_QR:", cv_img_qr.shape, cv_img_qr.dtype)
# gray = cv2. cvtColor(image, cv2. COLOR_BGR2GRAY)
# x_offset=y_offset=50
# scaledimg[y_offset:y_offset+cv_img_qr.shape[0], x_offset:x_offset+cv_img_qr.shape[1]] = cv_img_qr
-
-
+
if options.test:
cv2.imshow("image", final_image)
cv2.waitKey(1)
- if options.compressed:
+ if options.compressed:
msg = bridge.cv2_to_compressed_imgmsg(final_image)
+ msg.header = data.header
image_pub.publish(msg)
else:
msg = bridge.cv2_to_imgmsg(final_image)
+ msg.header = data.header
image_pub.publish(msg)
-
+
if options.decode:
retval, points = detector.detect(final_image)
- # print(retval, points)
+ print(retval, points)
if retval:
remembered_points = points
have_points = True
else:
print("FAILED TO FIND CODE, trying to use saved points")
+
if have_points:
decoded_data, _ = detector.decode(final_image, remembered_points)
if decoded_data:
print("DECODED DATA:", decoded_data)
if options.qr_transform:
- jobj = json.loads(decoded_data)
- transform = json_to_transform(jobj)
+ #jobj = json.loads(decoded_data)
+ #transform = json_to_transform(jobj)
print("ROS TRANSFORM:", transform)
else:
print("DECODING FAILED!!!!!!!!!")
@@ -186,13 +188,13 @@ def image_callback(data):
#print("DETECT:", retval, points)
#qr_timestamp, _ , _ = detector.detectAndDecode(final_image)
#print("QR_TIMESTAMP:", qr_timestamp)
-
+
except Exception as exc:
print("image_callback exception:", exc)
#fifo.close()
#fifo = open(options.fifo, 'wb')
#print(f'FIFO RE-OPENED: {options.fifo}')
-
+
if __name__ == "__main__":
rospy.init_node ("add_qr")
@@ -206,7 +208,7 @@ if __name__ == "__main__":
# os.mkfifo(options.fifo)
#except Exception as exc:
# print(f'{options.fifo} exist:', exc)
-
+
#if not options.test:
# fifo = open(options.fifo, 'wb')
# print(f'FIFO OPENED: {options.fifo}')
@@ -229,9 +231,9 @@ if __name__ == "__main__":
image_pub = rospy.Publisher(out_topic, CompressedImage, queue_size=10)
else:
image_sub = rospy.Subscriber(in_topic, Image, image_callback)
- image_pub = rospy.Publisher(out_topic, Image, queue_size=10)
+ image_pub = rospy.Publisher(out_topic, Image, queue_size=10)
+
+ rospy.spin()
- rospy.spin()
-
diff --git a/src/decode_qr_inference.py b/src/decode_qr_inference.py
new file mode 100755
index 0000000000000000000000000000000000000000..a7eb1290004efc82a26b5b6b91a77bbfd605ce01
--- /dev/null
+++ b/src/decode_qr_inference.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python3
+
+import rospy
+
+import json
+import time
+import os
+import qrcode
+import numpy
+from PIL import Image as PilImage
+
+from optparse import OptionParser
+
+from lrs_msgs_common.msg import InferenceVisionDetection
+
+import cv2
+from sensor_msgs.msg import Image, CompressedImage
+from cv_bridge import CvBridge, CvBridgeError
+import numpy as np
+import tf
+
+from camera_info_manager import CameraInfoManager, CameraInfoError, CameraInfoMissingError, resolveURL, parseURL, \
+ loadCalibrationFile, getPackageFileName, URL_empty, URL_file, URL_package
+
+remembered_points = None
+have_points = False
+camera_info = None
+
+parser = OptionParser()
+parser.add_option("", "--image-topic", action="store", type="string", dest="image_topic",
+ help="Topic to take image from", default="/dji21/camera0/image_raw_orig")
+parser.add_option("", "--camera-info-file", action="store", type="string", dest="camera_info_file",
+ help="Full path of the camera calibration YAML file", default="camera_gazebo.yml")
+parser.add_option("", "--camera-name", action="store", type="string", dest="camera_name",
+ help="Camera name", default="camera_gazebo")
+parser.add_option("", "--topic-out", action="store", type="string", dest="topic_out",
+ help="Info decoded from the image + camera info", default="inference_image_info")
+parser.add_option("", "--compressed", action="store_true", dest="compressed", help="If true image in is compressed.")
+parser.add_option("", "--test", action="store_true", dest="test", help="Test mode.")
+parser.add_option("", "--debug", action="store_true", dest="debug", help="Extra debug print")
+(options, args) = parser.parse_args()
+
+from geometry_msgs.msg import TransformStamped, Transform
+
+
+def json_to_transform(jobj):
+ res = Transform()
+ res.translation.x = jobj["tra"]["x"]
+ res.translation.y = jobj["tra"]["y"]
+ res.translation.z = jobj["tra"]["z"]
+ res.rotation.x = jobj["rot"]["x"]
+ res.rotation.y = jobj["rot"]["y"]
+ res.rotation.z = jobj["rot"]["z"]
+ res.rotation.w = jobj["rot"]["w"]
+ return res
+
+
+def image_callback(data):
+ global fifo, remembered_points, have_points, camera_info
+ try:
+ # print("image_callback_main")
+ if options.compressed:
+ img = bridge.compressed_imgmsg_to_cv2(data, desired_encoding='bgr8')
+ else:
+ #img = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
+ # FIXME - encoding gives an error
+ img = bridge.imgmsg_to_cv2(data)
+
+ scaledimg = cv2.resize(img, (1024, 600))
+
+ final_image = scaledimg
+
+ retval, points = detector.detect(final_image)
+ # print(retval, points)
+ if retval:
+ remembered_points = points
+ have_points = True
+ else:
+ print("FAILED TO FIND CODE, trying to use saved points")
+
+ if have_points:
+ decoded_data, _ = detector.decode(final_image, remembered_points)
+ if decoded_data:
+ print("DECODED DATA:", decoded_data)
+
+ jobj = json.loads(decoded_data)
+ c2w_transform = json_to_transform(jobj)
+ if options.debug:
+ print("ROS TRANSFORM:", c2w_transform)
+ else:
+ print("DECODING FAILED!!!!!!!!!")
+
+ # make and publish the message
+ #
+ msg = InferenceVisionDetection()
+ msg.header = data.header
+ msg.camera_info = camera_info
+ msg.c2w_transform = c2w_transform
+ data_pub.publish(msg)
+ if options.debug:
+ print(msg)
+
+ except Exception as exc:
+ print("image_callback exception:", exc)
+
+
+if __name__ == "__main__":
+
+ rospy.init_node("decode_qr_inference")
+
+ print("OpenCV version: ", cv2.__version__)
+
+ listener = tf.TransformListener()
+ detector = cv2.QRCodeDetector()
+
+ print("Create cv bridge")
+ bridge = CvBridge()
+
+ _camera_name = options.camera_name
+ _camera_info_file = options.camera_info_file
+
+ resolved_url = resolveURL(_camera_info_file, _camera_name)
+ print("Camera info URL: ", resolved_url)
+
+ # the exception is not working - wrong version?
+ try:
+ camera_info = loadCalibrationFile(_camera_info_file, _camera_name)
+ except IOError as exc:
+ print("Camera info load exception: ", exc)
+ exit()
+
+ # extra check because the above exception is not working
+ if camera_info.height != 0 and camera_info.width != 0:
+ print("Loaded camera info: ", camera_info)
+ else:
+ print("Loaded camera info width and height are zeros. Calibration camera info URL correct?")
+ exit()
+
+ if options.compressed:
+ in_topic = f'{options.image_topic}/compressed'
+ else:
+ in_topic = f'{options.image_topic}'
+
+ print("in_topic:", in_topic)
+ print("out_topic:", options.topic_out)
+
+ if options.compressed:
+ image_sub = rospy.Subscriber(in_topic, CompressedImage, image_callback)
+ else:
+ image_sub = rospy.Subscriber(in_topic, Image, image_callback)
+
+ data_pub = rospy.Publisher(options.topic_out, InferenceVisionDetection, queue_size=10)
+
+ rospy.spin()