ping360.py

#!/usr/bin/env python3

# ping360.py
# A device API for the Blue Robotics Ping360 scanning sonar

# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!
# THIS IS AN AUTOGENERATED FILE
# DO NOT EDIT
# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!

from brping import definitions
from brping import PingDevice
from brping import pingmessage
import serial
import time

class Ping360(PingDevice):
    def initialize(self):
        if not PingDevice.initialize(self):
            return False
        if (self.readDeviceInformation() is None):
            return False
        return True

    ##
    # @brief Get a auto_device_data message from the device\n
    # Message description:\n
    # Extended version of *device_data* with *auto_transmit* information. The sensor emits this message when in *auto_transmit* mode.
    #
    # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n
    # mode: Operating mode (1 for Ping360)\n
    # gain_setting: Analog gain setting (0 = low, 1 = normal, 2 = high)\n
    # angle: Units: gradian; Head angle\n
    # transmit_duration: Units: microsecond; Acoustic transmission duration (1~1000 microseconds)\n
    # sample_period: Time interval between individual signal intensity samples in 25nsec increments (80 to 40000 == 2 microseconds to 1000 microseconds)\n
    # transmit_frequency: Units: kHz; Acoustic operating frequency. Frequency range is 500kHz to 1000kHz, however it is only practical to use say 650kHz to 850kHz due to the narrow bandwidth of the acoustic receiver.\n
    # start_angle: Units: gradian; Head angle to begin scan sector for autoscan in gradians (0~399 = 0~360 degrees).\n
    # stop_angle: Units: gradian; Head angle to end scan sector for autoscan in gradians (0~399 = 0~360 degrees).\n
    # num_steps: Units: gradian; Number of 0.9 degree motor steps between pings for auto scan (1~10 = 0.9~9.0 degrees)\n
    # delay: Units: millisecond; An additional delay between successive transmit pulses (0~100 ms). This may be necessary for some programs to avoid collisions on the RS485 USRT.\n
    # number_of_samples: Number of samples per reflected signal\n
    # data: 8 bit binary data array representing sonar echo strength\n
    def get_auto_device_data(self):
        if self.request(definitions.PING360_AUTO_DEVICE_DATA) is None:
            return None
        data = ({
            "mode": self._mode,  # Operating mode (1 for Ping360)
            "gain_setting": self._gain_setting,  # Analog gain setting (0 = low, 1 = normal, 2 = high)
            "angle": self._angle,  # Units: gradian; Head angle
            "transmit_duration": self._transmit_duration,  # Units: microsecond; Acoustic transmission duration (1~1000 microseconds)
            "sample_period": self._sample_period,  # Time interval between individual signal intensity samples in 25nsec increments (80 to 40000 == 2 microseconds to 1000 microseconds)
            "transmit_frequency": self._transmit_frequency,  # Units: kHz; Acoustic operating frequency. Frequency range is 500kHz to 1000kHz, however it is only practical to use say 650kHz to 850kHz due to the narrow bandwidth of the acoustic receiver.
            "start_angle": self._start_angle,  # Units: gradian; Head angle to begin scan sector for autoscan in gradians (0~399 = 0~360 degrees).
            "stop_angle": self._stop_angle,  # Units: gradian; Head angle to end scan sector for autoscan in gradians (0~399 = 0~360 degrees).
            "num_steps": self._num_steps,  # Units: gradian; Number of 0.9 degree motor steps between pings for auto scan (1~10 = 0.9~9.0 degrees)
            "delay": self._delay,  # Units: millisecond; An additional delay between successive transmit pulses (0~100 ms). This may be necessary for some programs to avoid collisions on the RS485 USRT.
            "number_of_samples": self._number_of_samples,  # Number of samples per reflected signal
            "data": self._data,  # 8 bit binary data array representing sonar echo strength
        })
        return data

    ##
    # @brief Get a device_data message from the device\n
    # Message description:\n
    # This message is used to communicate the current sonar state. If the data field is populated, the other fields indicate the sonar state when the data was captured. The time taken before the response to the command is sent depends on the difference between the last angle scanned and the new angle in the parameters as well as the number of samples and sample interval (range). To allow for the worst case reponse time the command timeout should be set to 4000 msec.
    #
    # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n
    # mode: Operating mode (1 for Ping360)\n
    # gain_setting: Analog gain setting (0 = low, 1 = normal, 2 = high)\n
    # angle: Units: gradian; Head angle\n
    # transmit_duration: Units: microsecond; Acoustic transmission duration (1~1000 microseconds)\n
    # sample_period: Time interval between individual signal intensity samples in 25nsec increments (80 to 40000 == 2 microseconds to 1000 microseconds)\n
    # transmit_frequency: Units: kHz; Acoustic operating frequency. Frequency range is 500kHz to 1000kHz, however it is only practical to use say 650kHz to 850kHz due to the narrow bandwidth of the acoustic receiver.\n
    # number_of_samples: Number of samples per reflected signal\n
    # data: 8 bit binary data array representing sonar echo strength\n
    def get_device_data(self):
        if self.request(definitions.PING360_DEVICE_DATA) is None:
            return None
        data = ({
            "mode": self._mode,  # Operating mode (1 for Ping360)
            "gain_setting": self._gain_setting,  # Analog gain setting (0 = low, 1 = normal, 2 = high)
            "angle": self._angle,  # Units: gradian; Head angle
            "transmit_duration": self._transmit_duration,  # Units: microsecond; Acoustic transmission duration (1~1000 microseconds)
            "sample_period": self._sample_period,  # Time interval between individual signal intensity samples in 25nsec increments (80 to 40000 == 2 microseconds to 1000 microseconds)
            "transmit_frequency": self._transmit_frequency,  # Units: kHz; Acoustic operating frequency. Frequency range is 500kHz to 1000kHz, however it is only practical to use say 650kHz to 850kHz due to the narrow bandwidth of the acoustic receiver.
            "number_of_samples": self._number_of_samples,  # Number of samples per reflected signal
            "data": self._data,  # 8 bit binary data array representing sonar echo strength
        })
        return data

    ##
    # @brief Send a device_id message to the device\n
    # Message description:\n
    # Change the device id\n
    # Send the message to write the device parameters, then read the values back from the device\n
    #
    # @param id - Device ID (1-254). 0 and 255 are reserved.
    # @param reserved - reserved
    #
    # @return If verify is False, True on successful communication with the device. If verify is False, True if the new device parameters are verified to have been written correctly. False otherwise (failure to read values back or on verification failure)
    def device_id(self, id, reserved, verify=True):
        m = pingmessage.PingMessage(definitions.PING360_DEVICE_ID)
        m.id = id
        m.reserved = reserved
        m.pack_msg_data()
        self.write(m.msg_data)
        if self.request(definitions.PING360_DEVICE_ID) is None:
            return False
        # Read back the data and check that changes have been applied
        if (verify
                and (self._id != id or self._reserved != reserved)):
            return False
        return True  # success        m.id = id
        m.reserved = reserved
        m.pack_msg_data()
        self.write(m.msg_data)


    def control_auto_transmit(self, mode, gain_setting, transmit_duration, sample_period, transmit_frequency, number_of_samples, start_angle, stop_angle, num_steps, delay):
        m = pingmessage.PingMessage(definitions.PING360_AUTO_TRANSMIT)
        m.mode = mode
        m.gain_setting = gain_setting
        m.transmit_duration = transmit_duration
        m.sample_period = sample_period
        m.transmit_frequency = transmit_frequency
        m.number_of_samples = number_of_samples
        m.start_angle = start_angle
        m.stop_angle = stop_angle
        m.num_steps = num_steps
        m.delay = delay
        m.pack_msg_data()
        self.write(m.msg_data)

    def control_motor_off(self):
        m = pingmessage.PingMessage(definitions.PING360_MOTOR_OFF)
        m.pack_msg_data()
        self.write(m.msg_data)

    def control_reset(self, bootloader, reserved):
        m = pingmessage.PingMessage(definitions.PING360_RESET)
        m.bootloader = bootloader
        m.reserved = reserved
        m.pack_msg_data()
        self.write(m.msg_data)

    def control_transducer(self, mode, gain_setting, angle, transmit_duration, sample_period, transmit_frequency, number_of_samples, transmit, reserved):
        m = pingmessage.PingMessage(definitions.PING360_TRANSDUCER)
        m.mode = mode
        m.gain_setting = gain_setting
        m.angle = angle
        m.transmit_duration = transmit_duration
        m.sample_period = sample_period
        m.transmit_frequency = transmit_frequency
        m.number_of_samples = number_of_samples
        m.transmit = transmit
        m.reserved = reserved
        m.pack_msg_data()
        self.write(m.msg_data)


    def set_mode(self, mode):
        self.control_transducer(
            mode,
            self._gain_setting,
            self._angle,
            self._transmit_duration,
            self._sample_period,
            self._transmit_frequency,
            self._number_of_samples,
            0,
            0
        )
        return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0)

    def set_gain_setting(self, gain_setting):
        self.control_transducer(
            self._mode,
            gain_setting,
            self._angle,
            self._transmit_duration,
            self._sample_period,
            self._transmit_frequency,
            self._number_of_samples,
            0,
            0
        )
        return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0)

    def set_angle(self, angle):
        self.control_transducer(
            self._mode,
            self._gain_setting,
            angle,
            self._transmit_duration,
            self._sample_period,
            self._transmit_frequency,
            self._number_of_samples,
            0,
            0
        )
        return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0)

    def set_transmit_duration(self, transmit_duration):
        self.control_transducer(
            self._mode,
            self._gain_setting,
            self._angle,
            transmit_duration,
            self._sample_period,
            self._transmit_frequency,
            self._number_of_samples,
            0,
            0
        )
        return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0)

    def set_sample_period(self, sample_period):
        self.control_transducer(
            self._mode,
            self._gain_setting,
            self._angle,
            self._transmit_duration,
            sample_period,
            self._transmit_frequency,
            self._number_of_samples,
            0,
            0
        )
        return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0)

    def set_transmit_frequency(self, transmit_frequency):
        self.control_transducer(
            self._mode,
            self._gain_setting,
            self._angle,
            self._transmit_duration,
            self._sample_period,
            transmit_frequency,
            self._number_of_samples,
            0,
            0
        )
        return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0)

    def set_number_of_samples(self, number_of_samples):
        self.control_transducer(
            self._mode,
            self._gain_setting,
            self._angle,
            self._transmit_duration,
            self._sample_period,
            self._transmit_frequency,
            number_of_samples,
            0,
            0
        )
        return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0)


    def readDeviceInformation(self):
        return self.request(definitions.PING360_DEVICE_DATA)

    def transmitAngle(self, angle):
        self.control_transducer(
            0, # reserved
            self._gain_setting,
            angle,
            self._transmit_duration,
            self._sample_period,
            self._transmit_frequency,
            self._number_of_samples,
            1,
            0
        )
        return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0)
    
    def transmit(self):
        return self.transmitAngle(self._angle)

if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser(description="Ping python library example.")
    parser.add_argument('--device', action="store", required=False, type=str, help="Ping device port. E.g: /dev/ttyUSB0")
    parser.add_argument('--baudrate', action="store", type=int, default=115200, help="Ping device baudrate. E.g: 115200")
    parser.add_argument('--udp', action="store", required=False, type=str, help="Ping UDP server. E.g: 192.168.2.2:9092")
    args = parser.parse_args()
    if args.device is None and args.udp is None:
        parser.print_help()
        exit(1)

    p = Ping360()
    if args.device is not None:
        p.connect_serial(args.device, args.baudrate)
    elif args.udp is not None:
        (host, port) = args.udp.split(':')
        p.connect_udp(host, int(port))

    print("Initialized: %s" % p.initialize())

    print(p.set_transmit_frequency(800))
    print(p.set_sample_period(80))
    print(p.set_number_of_samples(200))

    tstart_s = time.time()
    for x in range(400):
        p.transmitAngle(x)
    tend_s = time.time()

    print(p)

    print("full scan in %dms, %dHz" % (1000*(tend_s - tstart_s), 400/(tend_s - tstart_s)))

    # turn on auto-scan with 1 grad steps
    p.control_auto_transmit(0,399,1,0)

    tstart_s = time.time()
    # wait for 400 device_data messages to arrive
    for x in range(400):
        p.wait_message([definitions.PING360_DEVICE_DATA])
    tend_s = time.time()

    print("full scan in %dms, %dHz" % (1000*(tend_s - tstart_s), 400/(tend_s - tstart_s)))

    # stop the auto-transmit process
    p.control_motor_off()

    # turn on auto-transmit with 10 grad steps
    p.control_auto_transmit(0,399,10,0)

    tstart_s = time.time()
    # wait for 40 device_data messages to arrive (40 * 10grad steps = 400 grads)
    for x in range(40):
        p.wait_message([definitions.PING360_DEVICE_DATA])
    tend_s = time.time()

    print("full scan in %dms, %dHz" % (1000*(tend_s - tstart_s), 400/(tend_s - tstart_s)))

    p.control_reset(0, 0)