Python Script to get measurements from Leica Disto S910 on Linux

leica_disto_measurement.py

#!/usr/bin/env python

# Authors: Wolf Vollprecht, Timon Homberger
#
# This script is able to obtain measurements from a Leica Disto S910
# from the WIFI HotSpot the Leica is creating. Upon pressing "Enter" the script will 
# trigger a measurement on the device and obtain the results on the computer.
#
# Basic instructions:
# 
#   - connect to Leica HotSpot
#   - run script
#   - done ... ;)
#
########################################################################################
# Licensed under the:
#
# The MIT License (MIT)
# Copyright (c) 2017
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:

#     The above copyright notice and this permission notice shall be included in all
#     copies or substantial portions of the Software.

#     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
#     EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
#     MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
#     IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
#     DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
#     OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE
#     OR OTHER DEALINGS IN THE SOFTWARE.

import socket
import math
import numpy as np

TCP_IP = '192.168.87.81'
TCP_PORT = 22222

BUFFER_SIZE = 10000

translation, rotation = None, None

m_num = 0

def get_rotation(alpha):
    return np.array([[np.cos(alpha), -np.sin(alpha), 0],
                     [np.sin(alpha),  np.cos(alpha), 0],
                     [            0,              0, 1]])


def get_measurement():
    global m_num
    global translation, rotation

    MESSAGE = b'get8\x0d'

    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.connect((TCP_IP, TCP_PORT))

    s.send(b'\x67\x62\x0d')
    data = '\n' + s.recv(BUFFER_SIZE)

    s.send(MESSAGE)
    data += '\n' + s.recv(BUFFER_SIZE)

    s.send(b'\x67\x62\x0d')
    data += '\n' + s.recv(BUFFER_SIZE)

    s.send(MESSAGE)
    data += '\n' + s.recv(BUFFER_SIZE)

    # this command obtains the gyro measurement (roll, pitch, yaw) (angles in radians)
    s.send(b'\x6d\x69\x0d')
    imu_measurement = s.recv(BUFFER_SIZE)

    elems = imu_measurement.split(' ')
    INC_deg = float(elems[2][3:]) * (180 / math.pi)

    # this command obtains HZ_upper, V_upper, HZ_lower, V_lower, distance, - 
    # (angles in radians, distance in meters)
    s.send(b'\x6d\x70\x0d')
    position = s.recv(BUFFER_SIZE)
    elems = position.split(' ')

    HZ_deg = (float(elems[1][3:]) + float(elems[3][3:])) / 2.0 * (180.0 / math.pi)
    V_deg = (float(elems[2][3:]) + float(elems[4][3:])) / 2.0 * (180.0 / math.pi)
    dist = float(elems[5][3:])

    V = (float(elems[2][3:]) + float(elems[4][3:])) / 2.0
    HZ = (float(elems[1][3:]) + float(elems[3][3:])) / 2.0

    # print(V, HZ, dist)

    # transform measurements from leica space to world space 
    d_hat = dist * math.cos(math.pi / 2 - V)
    z_hat = dist * math.sin(math.pi / 2 - V)

    x_l = d_hat * math.cos(HZ)
    y_l = d_hat * math.sin(HZ)
    
    measurement_transformed = np.array([x_l, y_l, z_hat])

    # select first measurement as origin
    if m_num == 0:
        translation = -np.array([x_l, y_l, z_hat])
        measurement_transformed += translation

    # select 2nd measurement as defining the x-axis
    elif m_num == 1:
        measurement_transformed += translation
        alpha = -np.arccos(np.dot(measurement_transformed, np.array([1, 0, 0])) /
                                  (np.linalg.norm(measurement_transformed)))
        rotation = get_rotation(alpha)
        measurement_transformed = np.dot(rotation, measurement_transformed)

    else:
        measurement_transformed += translation
        measurement_transformed = np.dot(rotation, measurement_transformed) 

    print("New measurement: %r" % measurement_transformed)

    m_num += 1
    
    # print("HZ: %f\nV:  %f\nD:  %f" % (HZ_deg, V_deg, dist))

    s.send(b'\x6d\x62\x0d')
    data += '\n' + s.recv(BUFFER_SIZE)

    # print(data)
    s.close()   

while True:
    raw_input("Press enter to get measurement...")
    get_measurement()