diff --git a/src/add_qr.py b/src/add_qr.py index f115d494696da8ee1cd3a2b6704446ce6465d67b..095a8a3e1ae1db770956f3dd37a5d98857745619 100755 --- a/src/add_qr.py +++ b/src/add_qr.py @@ -37,12 +37,12 @@ from geometry_msgs.msg import TransformStamped, Transform def transform_to_json(tr): jobj = { - "translation": { + "tra": { "x": tr.translation.x, "y": tr.translation.y, "z": tr.translation.z }, - "rotation": { + "rot": { "x": tr.rotation.x, "y": tr.rotation.y, "z": tr.rotation.z, @@ -51,6 +51,17 @@ def transform_to_json(tr): } return jobj +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 header_to_json(data): jobj = { "seq": data.seq, @@ -164,6 +175,10 @@ def image_callback(data): 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) + print("ROS TRANSFORM:", transform) else: print("DECODING FAILED!!!!!!!!!")