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

Work on qr code.

parent cabdd7ba
No related branches found
No related tags found
No related merge requests found
...@@ -15,15 +15,38 @@ import cv2 ...@@ -15,15 +15,38 @@ import cv2
from sensor_msgs.msg import Image, CompressedImage from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge, CvBridgeError from cv_bridge import CvBridge, CvBridgeError
import numpy as np import numpy as np
import tf
parser = OptionParser() 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 ("", "--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 ("", "--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 ("", "--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 ("", "--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() (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): def header_to_json(data):
jobj = { jobj = {
"seq": data.seq, "seq": data.seq,
...@@ -49,13 +72,40 @@ def image_callback(data): ...@@ -49,13 +72,40 @@ def image_callback(data):
else: else:
img = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8') img = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
# print(data.header) if options.qr_header:
jobj = header_to_json(data.header) # print(data.header)
print(json.dumps(jobj)) 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)) 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) # print("SCALEDIMG:", scaledimg.shape, scaledimg.dtype)
frame_pil = PilImage.fromarray(scaledimg) frame_pil = PilImage.fromarray(scaledimg)
...@@ -92,9 +142,13 @@ def image_callback(data): ...@@ -92,9 +142,13 @@ def image_callback(data):
if options.compressed: if options.compressed:
msg = bridge.cv2_to_compressed_imgmsg(final_image) msg = bridge.cv2_to_compressed_imgmsg(final_image)
image_pub.publish(msg) image_pub.publish(msg)
else:
msg = bridge.cv2_to_imgmsg(final_image)
image_pub.publish(msg)
decoded_data, _ , _ = detector.detectAndDecode(final_image) if options.decode:
print("DECODED DATA:", decoded_data) decoded_data, _ , _ = detector.detectAndDecode(final_image)
print("DECODED DATA:", decoded_data)
#retval, points = detector.detect(final_image) #retval, points = detector.detect(final_image)
...@@ -112,6 +166,7 @@ if __name__ == "__main__": ...@@ -112,6 +166,7 @@ if __name__ == "__main__":
rospy.init_node ("add_qr") rospy.init_node ("add_qr")
listener = tf.TransformListener()
detector = cv2.QRCodeDetector() detector = cv2.QRCodeDetector()
#try: #try:
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment