Skip to content
Snippets Groups Projects
Commit 41bd8f00 authored by Tommy Persson's avatar Tommy Persson
Browse files

Work on qr code.

parent cabdd7ba
Branches
Tags
No related merge requests found
......@@ -15,15 +15,38 @@ import cv2
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import tf
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 ("", "--compressed", action="store_true", dest="compressed", help="If true image in is compressed.")
parser.add_option ("", "--decode", action="store_true", dest="decode", help="If true test the decoding.")
#parser.add_option ("", "--fifo", action="store", type="string", dest="fifo", help="Fifo to write raw images to", default="/tmp/fifo_dji21")
parser.add_option ("", "--test", action="store_true", dest="test", help="Test mode.")
parser.add_option ("", "--qr", action="store_true", dest="qr", help="Time encoding qr code.")
parser.add_option ("", "--qr-transform", action="store_true", dest="qr_transform", help="Time encoding qr code.")
parser.add_option ("", "--qr-header", action="store_true", dest="qr_header", help="Time encoding qr code.")
parser.add_option ("", "--qr-time", action="store_true", dest="qr_time", help="Time encoding qr code.")
(options, args) = parser.parse_args()
from geometry_msgs.msg import TransformStamped, Transform
def transform_to_json(tr):
jobj = {
"translation": {
"x": tr.translation.x,
"y": tr.translation.y,
"z": tr.translation.z
},
"rotation": {
"x": tr.rotation.x,
"y": tr.rotation.y,
"z": tr.rotation.z,
"w": tr.rotation.w
}
}
return jobj
def header_to_json(data):
jobj = {
"seq": data.seq,
......@@ -49,13 +72,40 @@ def image_callback(data):
else:
img = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
# print(data.header)
jobj = header_to_json(data.header)
print(json.dumps(jobj))
if options.qr_header:
# print(data.header)
jobj = header_to_json(data.header)
# print(json.dumps(jobj))
look_up_time = rospy.Time(0)
# look_up_time = data.header.stamp
# print("TRANS-ROT:", trans, rot)
if options.qr_transform:
try:
(trans,rot) = listener.lookupTransform('/world', '/dji0/camera0/image_frame', look_up_time)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("Exception lookup trans:", look_up_time)
return
transform = Transform()
transform.translation.x = round(trans[0], 3)
transform.translation.y = round(trans[1], 3)
transform.translation.z = round(trans[2], 3)
transform.rotation.x = round(rot[0], 5)
transform.rotation.y = round(rot[1], 5)
transform.rotation.z = round(rot[2], 5)
transform.rotation.w = round(rot[3], 5)
# print(transform)
jobj = transform_to_json(transform)
# print("DUMPS:", json.dumps(jobj))
scaledimg = cv2.resize(img, (1024, 600))
if options.qr:
if options.qr_transform or options.qr_header or options.qr_time:
# print("SCALEDIMG:", scaledimg.shape, scaledimg.dtype)
frame_pil = PilImage.fromarray(scaledimg)
......@@ -92,9 +142,13 @@ def image_callback(data):
if options.compressed:
msg = bridge.cv2_to_compressed_imgmsg(final_image)
image_pub.publish(msg)
else:
msg = bridge.cv2_to_imgmsg(final_image)
image_pub.publish(msg)
decoded_data, _ , _ = detector.detectAndDecode(final_image)
print("DECODED DATA:", decoded_data)
if options.decode:
decoded_data, _ , _ = detector.detectAndDecode(final_image)
print("DECODED DATA:", decoded_data)
#retval, points = detector.detect(final_image)
......@@ -112,6 +166,7 @@ if __name__ == "__main__":
rospy.init_node ("add_qr")
listener = tf.TransformListener()
detector = cv2.QRCodeDetector()
#try:
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment