From c8e2b8b9917ee6ec323a712be3eaa2faa7a2d9c1 Mon Sep 17 00:00:00 2001 From: Stormiix Date: Thu, 5 Dec 2019 15:46:40 +0100 Subject: [PATCH 1/7] OpenCV Ping360 --- .gitignore | 2 + src/ping360/__init__.py | 7 +- src/ping360/definitions.py | 444 ++++++++++++++++++++++++++++++++++++ src/ping360/device.py | 233 +++++++++++++++++++ src/ping360/pingmessage.py | 449 +++++++++++++++++++++++++++++++++++++ src/ping360/polarplot.py | 71 +++++- src/ping360/sensor.py | 219 ++++++++++++++++++ 7 files changed, 1421 insertions(+), 4 deletions(-) create mode 100644 .gitignore create mode 100644 src/ping360/definitions.py create mode 100644 src/ping360/device.py create mode 100644 src/ping360/pingmessage.py create mode 100644 src/ping360/sensor.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d28ff66 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.vscode +**/*.pyc \ No newline at end of file diff --git a/src/ping360/__init__.py b/src/ping360/__init__.py index 001a5e7..0dd58c4 100644 --- a/src/ping360/__init__.py +++ b/src/ping360/__init__.py @@ -1 +1,6 @@ -from polarplot import main \ No newline at end of file +from . import polarplot.main as main +from brping.definitions import * +from brping.pingmessage import * +from brping.device import PingDevice +from brping.ping1d import Ping1D +from brping.ping360 import Ping360 \ No newline at end of file diff --git a/src/ping360/definitions.py b/src/ping360/definitions.py new file mode 100644 index 0000000..c7585eb --- /dev/null +++ b/src/ping360/definitions.py @@ -0,0 +1,444 @@ +COMMON_ACK = 1 +COMMON_NACK = 2 +COMMON_ASCII_TEXT = 3 +COMMON_GENERAL_REQUEST = 6 +COMMON_DEVICE_INFORMATION = 4 +COMMON_PROTOCOL_VERSION = 5 + +# variable length fields are formatted with 's', and always occur at the end of the payload +# the format string for these messages is adjusted at runtime, and 's' inserted appropriately at runtime +# see PingMessage.get_payload_format() +payload_dict_common = { + COMMON_ACK: { + "name": "ack", + "format": "H", + "field_names": ( + "acked_id", + ), + "payload_length": 2 + }, + + COMMON_NACK: { + "name": "nack", + "format": "H", + "field_names": ( + "nacked_id", + "nack_message", + ), + "payload_length": 2 + }, + + COMMON_ASCII_TEXT: { + "name": "ascii_text", + "format": "", + "field_names": ( + "ascii_message", + ), + "payload_length": 0 + }, + + COMMON_GENERAL_REQUEST: { + "name": "general_request", + "format": "H", + "field_names": ( + "requested_id", + ), + "payload_length": 2 + }, + + COMMON_DEVICE_INFORMATION: { + "name": "device_information", + "format": "BBBBBB", + "field_names": ( + "device_type", + "device_revision", + "firmware_version_major", + "firmware_version_minor", + "firmware_version_patch", + "reserved", + ), + "payload_length": 6 + }, + + COMMON_PROTOCOL_VERSION: { + "name": "protocol_version", + "format": "BBBB", + "field_names": ( + "version_major", + "version_minor", + "version_patch", + "reserved", + ), + "payload_length": 4 + }, + +} + +PING1D_SET_DEVICE_ID = 1000 +PING1D_SET_RANGE = 1001 +PING1D_SET_SPEED_OF_SOUND = 1002 +PING1D_SET_MODE_AUTO = 1003 +PING1D_SET_PING_INTERVAL = 1004 +PING1D_SET_GAIN_SETTING = 1005 +PING1D_SET_PING_ENABLE = 1006 +PING1D_FIRMWARE_VERSION = 1200 +PING1D_DEVICE_ID = 1201 +PING1D_VOLTAGE_5 = 1202 +PING1D_SPEED_OF_SOUND = 1203 +PING1D_RANGE = 1204 +PING1D_MODE_AUTO = 1205 +PING1D_PING_INTERVAL = 1206 +PING1D_GAIN_SETTING = 1207 +PING1D_TRANSMIT_DURATION = 1208 +PING1D_GENERAL_INFO = 1210 +PING1D_DISTANCE_SIMPLE = 1211 +PING1D_DISTANCE = 1212 +PING1D_PROCESSOR_TEMPERATURE = 1213 +PING1D_PCB_TEMPERATURE = 1214 +PING1D_PING_ENABLE = 1215 +PING1D_PROFILE = 1300 +PING1D_GOTO_BOOTLOADER = 1100 +PING1D_CONTINUOUS_START = 1400 +PING1D_CONTINUOUS_STOP = 1401 + +# variable length fields are formatted with 's', and always occur at the end of the payload +# the format string for these messages is adjusted at runtime, and 's' inserted appropriately at runtime +# see PingMessage.get_payload_format() +payload_dict_ping1d = { + PING1D_SET_DEVICE_ID: { + "name": "set_device_id", + "format": "B", + "field_names": ( + "device_id", + ), + "payload_length": 1 + }, + + PING1D_SET_RANGE: { + "name": "set_range", + "format": "II", + "field_names": ( + "scan_start", + "scan_length", + ), + "payload_length": 8 + }, + + PING1D_SET_SPEED_OF_SOUND: { + "name": "set_speed_of_sound", + "format": "I", + "field_names": ( + "speed_of_sound", + ), + "payload_length": 4 + }, + + PING1D_SET_MODE_AUTO: { + "name": "set_mode_auto", + "format": "B", + "field_names": ( + "mode_auto", + ), + "payload_length": 1 + }, + + PING1D_SET_PING_INTERVAL: { + "name": "set_ping_interval", + "format": "H", + "field_names": ( + "ping_interval", + ), + "payload_length": 2 + }, + + PING1D_SET_GAIN_SETTING: { + "name": "set_gain_setting", + "format": "B", + "field_names": ( + "gain_setting", + ), + "payload_length": 1 + }, + + PING1D_SET_PING_ENABLE: { + "name": "set_ping_enable", + "format": "B", + "field_names": ( + "ping_enabled", + ), + "payload_length": 1 + }, + + PING1D_FIRMWARE_VERSION: { + "name": "firmware_version", + "format": "BBHH", + "field_names": ( + "device_type", + "device_model", + "firmware_version_major", + "firmware_version_minor", + ), + "payload_length": 6 + }, + + PING1D_DEVICE_ID: { + "name": "device_id", + "format": "B", + "field_names": ( + "device_id", + ), + "payload_length": 1 + }, + + PING1D_VOLTAGE_5: { + "name": "voltage_5", + "format": "H", + "field_names": ( + "voltage_5", + ), + "payload_length": 2 + }, + + PING1D_SPEED_OF_SOUND: { + "name": "speed_of_sound", + "format": "I", + "field_names": ( + "speed_of_sound", + ), + "payload_length": 4 + }, + + PING1D_RANGE: { + "name": "range", + "format": "II", + "field_names": ( + "scan_start", + "scan_length", + ), + "payload_length": 8 + }, + + PING1D_MODE_AUTO: { + "name": "mode_auto", + "format": "B", + "field_names": ( + "mode_auto", + ), + "payload_length": 1 + }, + + PING1D_PING_INTERVAL: { + "name": "ping_interval", + "format": "H", + "field_names": ( + "ping_interval", + ), + "payload_length": 2 + }, + + PING1D_GAIN_SETTING: { + "name": "gain_setting", + "format": "I", + "field_names": ( + "gain_setting", + ), + "payload_length": 4 + }, + + PING1D_TRANSMIT_DURATION: { + "name": "transmit_duration", + "format": "H", + "field_names": ( + "transmit_duration", + ), + "payload_length": 2 + }, + + PING1D_GENERAL_INFO: { + "name": "general_info", + "format": "HHHHBB", + "field_names": ( + "firmware_version_major", + "firmware_version_minor", + "voltage_5", + "ping_interval", + "gain_setting", + "mode_auto", + ), + "payload_length": 10 + }, + + PING1D_DISTANCE_SIMPLE: { + "name": "distance_simple", + "format": "IB", + "field_names": ( + "distance", + "confidence", + ), + "payload_length": 5 + }, + + PING1D_DISTANCE: { + "name": "distance", + "format": "IHHIIII", + "field_names": ( + "distance", + "confidence", + "transmit_duration", + "ping_number", + "scan_start", + "scan_length", + "gain_setting", + ), + "payload_length": 24 + }, + + PING1D_PROCESSOR_TEMPERATURE: { + "name": "processor_temperature", + "format": "H", + "field_names": ( + "processor_temperature", + ), + "payload_length": 2 + }, + + PING1D_PCB_TEMPERATURE: { + "name": "pcb_temperature", + "format": "H", + "field_names": ( + "pcb_temperature", + ), + "payload_length": 2 + }, + + PING1D_PING_ENABLE: { + "name": "ping_enable", + "format": "B", + "field_names": ( + "ping_enabled", + ), + "payload_length": 1 + }, + + PING1D_PROFILE: { + "name": "profile", + "format": "IHHIIIIH", + "field_names": ( + "distance", + "confidence", + "transmit_duration", + "ping_number", + "scan_start", + "scan_length", + "gain_setting", + "profile_data_length", + "profile_data", + ), + "payload_length": 26 + }, + + PING1D_GOTO_BOOTLOADER: { + "name": "goto_bootloader", + "format": "", + "field_names": ( + ), + "payload_length": 0 + }, + + PING1D_CONTINUOUS_START: { + "name": "continuous_start", + "format": "H", + "field_names": ( + "id", + ), + "payload_length": 2 + }, + + PING1D_CONTINUOUS_STOP: { + "name": "continuous_stop", + "format": "H", + "field_names": ( + "id", + ), + "payload_length": 2 + }, + +} + +PING360_DEVICE_ID = 2000 +PING360_DEVICE_DATA = 2300 +PING360_RESET = 2600 +PING360_TRANSDUCER = 2601 + +# variable length fields are formatted with 's', and always occur at the end of the payload +# the format string for these messages is adjusted at runtime, and 's' inserted appropriately at runtime +# see PingMessage.get_payload_format() +payload_dict_ping360 = { + PING360_DEVICE_ID: { + "name": "device_id", + "format": "BB", + "field_names": ( + "id", + "reserved", + ), + "payload_length": 2 + }, + + PING360_DEVICE_DATA: { + "name": "device_data", + "format": "BBHHHHHH", + "field_names": ( + "mode", + "gain_setting", + "angle", + "transmit_duration", + "sample_period", + "transmit_frequency", + "number_of_samples", + "data_length", + "data", + ), + "payload_length": 14 + }, + + PING360_RESET: { + "name": "reset", + "format": "BB", + "field_names": ( + "bootloader", + "reserved", + ), + "payload_length": 2 + }, + + PING360_TRANSDUCER: { + "name": "transducer", + "format": "BBHHHHHBB", + "field_names": ( + "mode", + "gain_setting", + "angle", + "transmit_duration", + "sample_period", + "transmit_frequency", + "number_of_samples", + "transmit", + "reserved", + ), + "payload_length": 14 + }, + +} + +PINGMESSAGE_UNDEFINED = 0 +payload_dict_all = { + PINGMESSAGE_UNDEFINED: { + "name": "undefined", + "format": "", + "field_names": (), + "payload_length": 0 + }, +} +payload_dict_all.update(payload_dict_common) +payload_dict_all.update(payload_dict_ping1d) +payload_dict_all.update(payload_dict_ping360) diff --git a/src/ping360/device.py b/src/ping360/device.py new file mode 100644 index 0000000..0f7632b --- /dev/null +++ b/src/ping360/device.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python3 + +# device.py +# A device API for devices implementing Blue Robotics ping-protocol + +# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! +# THIS IS AN AUTOGENERATED FILE +# DO NOT EDIT +# ~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~!~! + +from brping import definitions +from brping import pingmessage +from collections import deque +import serial +import time + +class PingDevice(object): + + _input_buffer = deque() + def __init__(self, device_name, baudrate=115200): + if device_name is None: + print("Device name is required") + return + + try: + print("Opening %s at %d bps" % (device_name, baudrate)) + + ## Serial object for device communication + self.iodev = serial.Serial(device_name, baudrate) + self.iodev.send_break() + self.iodev.write("U".encode("utf-8")) + + except Exception as e: + print("Failed to open the given serial port") + print("\t", e) + exit(1) + + ## A helper class to take care of decoding the input stream + self.parser = pingmessage.PingParser() + + ## device id of this Ping1D object, used for dst_device_id in outgoing messages + self.my_id = 255 + + ## + # @brief Consume rx buffer data until a new message is successfully decoded + # + # @return A new PingMessage: as soon as a message is parsed (there may be data remaining in the buffer to be parsed, thus requiring subsequent calls to read()) + # @return None: if the buffer is empty and no message has been parsed + def read(self): + bytes = self.iodev.read(self.iodev.in_waiting) + self._input_buffer.extendleft(bytes) + + while len(self._input_buffer): + b = self._input_buffer.pop() + + if self.parser.parse_byte(b) == pingmessage.PingParser.NEW_MESSAGE: + # a successful read depends on a successful handling + if not self.handle_message(self.parser.rx_msg): + return None + else: + return self.parser.rx_msg + return None + + ## + # @brief Write data to device + # + # @param data: bytearray to write to device + # + # @return Number of bytes written + def write(self, data): + return self.iodev.write(data) + + ## + # @brief Make sure there is a device on and read some initial data + # + # @return True if the device replies with expected data, False otherwise + def initialize(self): + if (self.request(definitions.COMMON_PROTOCOL_VERSION) is None): + return False + return True + + ## + # @brief Request the given message ID + # + # @param m_id: The message ID to request from the device + # @param timeout: The time in seconds to wait for the device to send + # the requested message before timing out and returning + # + # @return PingMessage: the device reply if it is received within timeout period, None otherwise + # + # @todo handle nack to exit without blocking + def request(self, m_id, timeout=0.5): + msg = pingmessage.PingMessage(definitions.COMMON_GENERAL_REQUEST) + msg.requested_id = m_id + msg.pack_msg_data() + self.write(msg.msg_data) + + # uncomment to return nacks in addition to m_id + # return self.wait_message([m_id, definitions.COMMON_NACK], timeout) + + return self.wait_message([m_id], timeout) + + ## + # @brief Wait until we receive a message from the device with the desired message_id for timeout seconds + # + # @param message_id: The message id to wait to receive from the device + # @param timeout: The timeout period in seconds to wait + # + # @return PingMessage: the message from the device if it is received within timeout period, None otherwise + def wait_message(self, message_ids, timeout=0.5): + tstart = time.time() + while time.time() < tstart + timeout: + msg = self.read() + if msg is not None: + if msg.message_id in message_ids: + return msg + time.sleep(0.005) + return None + + ## + # @brief Handle an incoming message from the device. + # Extract message fields into self attributes. + # + # @param msg: the PingMessage to handle. + # @return True if the PingMessage was handled successfully + def handle_message(self, msg): + # TODO is this message for us? + setattr(self, "_src_device_id", msg.src_device_id) + setattr(self, "_dst_device_id", msg.dst_device_id) + + if msg.message_id in pingmessage.payload_dict: + try: + for attr in pingmessage.payload_dict[msg.message_id]["field_names"]: + setattr(self, "_" + attr, getattr(msg, attr)) + except AttributeError as e: + print("attribute error while handling msg %d (%s): %s" % (msg.message_id, msg.name, msg.msg_data)) + return False + else: + print("Unrecognized message: %d", msg) + return False + + return True + + ## + # @brief Dump object into string representation. + # + # @return string: a string representation of the object + def __repr__(self): + representation = "---------------------------------------------------------\n~Ping1D Object~" + + attrs = vars(self) + for attr in sorted(attrs): + try: + if attr != 'iodev': + representation += "\n - " + attr + "(hex): " + str([hex(item) for item in getattr(self, attr)]) + if attr != 'data': + representation += "\n - " + attr + "(string): " + str(getattr(self, attr)) + # TODO: Better filter this exception + except: + representation += "\n - " + attr + ": " + str(getattr(self, attr)) + return representation + + ## + # @brief Get a device_information message from the device\n + # Message description:\n + # Device information + # + # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n + # device_type: Device type. 0: Unknown; 1: Ping Echosounder; 2: Ping360\n + # device_revision: device-specific hardware revision\n + # firmware_version_major: Firmware version major number.\n + # firmware_version_minor: Firmware version minor number.\n + # firmware_version_patch: Firmware version patch number.\n + # reserved: reserved\n + def get_device_information(self): + if self.request(definitions.COMMON_DEVICE_INFORMATION) is None: + return None + data = ({ + "device_type": self._device_type, # Device type. 0: Unknown; 1: Ping Echosounder; 2: Ping360 + "device_revision": self._device_revision, # device-specific hardware revision + "firmware_version_major": self._firmware_version_major, # Firmware version major number. + "firmware_version_minor": self._firmware_version_minor, # Firmware version minor number. + "firmware_version_patch": self._firmware_version_patch, # Firmware version patch number. + "reserved": self._reserved, # reserved + }) + return data + + ## + # @brief Get a protocol_version message from the device\n + # Message description:\n + # The protocol version + # + # @return None if there is no reply from the device, otherwise a dictionary with the following keys:\n + # version_major: Protocol version major number.\n + # version_minor: Protocol version minor number.\n + # version_patch: Protocol version patch number.\n + # reserved: reserved\n + def get_protocol_version(self): + if self.request(definitions.COMMON_PROTOCOL_VERSION) is None: + return None + data = ({ + "version_major": self._version_major, # Protocol version major number. + "version_minor": self._version_minor, # Protocol version minor number. + "version_patch": self._version_patch, # Protocol version patch number. + "reserved": self._reserved, # reserved + }) + return data + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser(description="Ping python library example.") + parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.") + parser.add_argument('--baudrate', action="store", type=int, default=115200, help="Ping device baudrate.") + args = parser.parse_args() + + p = PingDevice(args.device, args.baudrate) + + print("Initialized: %s" % p.initialize()) + + print("\ntesting get_device_information") + result = p.get_device_information() + print(" " + str(result)) + print(" > > pass: %s < <" % (result is not None)) + + print("\ntesting get_protocol_version") + result = p.get_protocol_version() + print(" " + str(result)) + print(" > > pass: %s < <" % (result is not None)) + + + print(p) \ No newline at end of file diff --git a/src/ping360/pingmessage.py b/src/ping360/pingmessage.py new file mode 100644 index 0000000..a6ce601 --- /dev/null +++ b/src/ping360/pingmessage.py @@ -0,0 +1,449 @@ +#!/usr/bin/env python3 + +# PingMessage.py +# Python implementation of the Blue Robotics 'Ping' binary message protocol + +import struct +from brping import definitions +payload_dict = definitions.payload_dict_all +asciiMsgs = [definitions.COMMON_NACK, definitions.COMMON_ASCII_TEXT] +variable_msgs = [definitions.PING1D_PROFILE, definitions.PING360_DEVICE_DATA, ] + + +class PingMessage(object): + ## header start byte 1 + start_1 = ord("B") + + ## header start byte 2 + start_2 = ord("R") + + ## header struct format + header_format = "BBHHBB" + + ## checksum struct format + checksum_format = "H" + + ## data endianness for struct formatting + endianess = "<" + + ## names of the header fields + header_field_names = ( + "start_1", + "start_2", + "payload_length", + "message_id", + "src_device_id", + "dst_device_id") + + ## number of bytes in a header + headerLength = 8 + + ## number of bytes in a checksum + checksumLength = 2 + + ## Messge constructor + # + # @par Ex request: + # @code + # m = PingMessage() + # m.request_id = m_id + # m.pack_msg_data() + # write(m.msg_data) + # @endcode + # + # @par Ex set: + # @code + # m = PingMessage(PING1D_SET_RANGE) + # m.start_mm = 1000 + # m.length_mm = 2000 + # m.update_checksum() + # write(m.msg_data) + # @endcode + # + # @par Ex receive: + # @code + # m = PingMessage(rxByteArray) + # if m.message_id == PING1D_RANGE + # start_mm = m.start_mm + # length_mm = m.length_mm + # @endcode + def __init__(self, msg_id=0, msg_data=None): + ## The message id + self.message_id = msg_id + + ## The request id for request messages + self.request_id = None + + ## The message destination + self.dst_device_id = 0 + ## The message source + self.src_device_id = 0 + ## The message checksum + self.checksum = 0 + + ## The raw data buffer for this message + # update with pack_msg_data() + self.msg_data = None + + # Constructor 1: make a pingmessage object from a binary data buffer + # (for receiving + unpacking) + if msg_data is not None: + self.unpack_msg_data(msg_data) + # Constructor 2: make a pingmessage object cooresponding to a message + # id, with field members ready to access and populate + # (for packing + transmitting) + else: + + try: + ## The name of this message + self.name = payload_dict[self.message_id]["name"] + + ## The field names of this message + self.payload_field_names = payload_dict[self.message_id]["field_names"] + + # initialize payload field members + for attr in self.payload_field_names: + setattr(self, attr, 0) + + # initialize vector fields + if self.message_id in variable_msgs: + setattr(self, self.payload_field_names[-1], bytearray()) + + ## Number of bytes in the message payload + self.update_payload_length() + + ## The struct formatting string for the message payload + self.payload_format = self.get_payload_format() + + # TODO handle better here, and catch Constructor 1 also + except KeyError as e: + print("message id not recognized: %d" % self.message_id, msg_data) + raise e + + ## Pack object attributes into self.msg_data (bytearray) + # @return self.msg_data + def pack_msg_data(self): + # necessary for variable length payloads + # update using current contents for the variable length field + self.update_payload_length() + + # Prepare struct packing format string + msg_format = PingMessage.endianess + PingMessage.header_format + self.get_payload_format() + + # Prepare complete list of field names (header + payload) + attrs = PingMessage.header_field_names + payload_dict[self.message_id]["field_names"] + + # Prepare iterable ordered list of values to pack + values = [] + for attr in attrs: + # this is a hack for requests + if attr == "message_id" and self.request_id is not None: + values.append(self.request_id) + else: + values.append(getattr(self, attr)) + + # Pack message contents into bytearray + self.msg_data = bytearray(struct.pack(msg_format, *values)) + + # Update and append checksum + self.msg_data += bytearray(struct.pack(PingMessage.endianess + PingMessage.checksum_format, self.update_checksum())) + + return self.msg_data + + ## Unpack a bytearray into object attributes + def unpack_msg_data(self, msg_data): + self.msg_data = msg_data + + # Extract header + header = struct.unpack(PingMessage.endianess + PingMessage.header_format, self.msg_data[0:PingMessage.headerLength]) + + for i, attr in enumerate(PingMessage.header_field_names): + setattr(self, attr, header[i]) + + ## The name of this message + self.name = payload_dict[self.message_id]["name"] + + ## The field names of this message + self.payload_field_names = payload_dict[self.message_id]["field_names"] + + if self.payload_length > 0: + ## The struct formatting string for the message payload + self.payload_format = self.get_payload_format() + + # Extract payload + try: + payload = struct.unpack(PingMessage.endianess + self.payload_format, self.msg_data[PingMessage.headerLength:PingMessage.headerLength + self.payload_length]) + except Exception as e: + print("error unpacking payload: %s" % e) + print("msg_data: %s, header: %s" % (msg_data, header)) + print("format: %s, buf: %s" % (PingMessage.endianess + self.payload_format, self.msg_data[PingMessage.headerLength:PingMessage.headerLength + self.payload_length])) + print(self.payload_format) + else: # only use payload if didn't raise exception + for i, attr in enumerate(self.payload_field_names): + try: + setattr(self, attr, payload[i]) + # empty trailing variable data field + except IndexError as e: + if self.message_id in variable_msgs: + setattr(self, attr, bytearray()) + pass + + # Extract checksum + self.checksum = struct.unpack(PingMessage.endianess + PingMessage.checksum_format, self.msg_data[PingMessage.headerLength + self.payload_length: PingMessage.headerLength + self.payload_length + PingMessage.checksumLength])[0] + + ## Calculate the checksum from the internal bytearray self.msg_data + def calculate_checksum(self): + checksum = 0 + for byte in self.msg_data[0:PingMessage.headerLength + self.payload_length]: + checksum += byte + return checksum + + ## Update the object checksum value + # @return the object checksum value + def update_checksum(self): + self.checksum = self.calculate_checksum() + return self.checksum + + ## Verify that the object checksum attribute is equal to the checksum calculated according to the internal bytearray self.msg_data + def verify_checksum(self): + return self.checksum == self.calculate_checksum() + + ## Update the payload_length attribute with the **current** payload length, including dynamic length fields (if present) + def update_payload_length(self): + if self.message_id in variable_msgs or self.message_id in asciiMsgs: + # The last field self.payload_field_names[-1] is always the single dynamic-length field + self.payload_length = payload_dict[self.message_id]["payload_length"] + len(getattr(self, self.payload_field_names[-1])) + else: + self.payload_length = payload_dict[self.message_id]["payload_length"] + + ## Get the python struct formatting string for the message payload + # @return the payload struct format string + def get_payload_format(self): + # messages with variable length fields + if self.message_id in variable_msgs or self.message_id in asciiMsgs: + var_length = self.payload_length - payload_dict[self.message_id]["payload_length"] # Subtract static length portion from payload length + if var_length <= 0: + return payload_dict[self.message_id]["format"] # variable data portion is empty + + return payload_dict[self.message_id]["format"] + str(var_length) + "s" + else: # messages with a static (constant) length + return payload_dict[self.message_id]["format"] + + ## Dump object into string representation + # @return string representation of the object + def __repr__(self): + header_string = "Header:" + for attr in PingMessage.header_field_names: + header_string += " " + attr + ": " + str(getattr(self, attr)) + + if self.payload_length == 0: # this is a hack/guard for empty body requests + payload_string = "" + else: + payload_string = "Payload:" + + # handle variable length messages + if self.message_id in variable_msgs: + + # static fields are handled as usual + for attr in payload_dict[self.message_id]["field_names"][:-1]: + payload_string += "\n - " + attr + ": " + str(getattr(self, attr)) + + # the variable length field is always the last field + attr = payload_dict[self.message_id]["field_names"][-1:][0] + + # format this field as a list of hex values (rather than a string if we did not perform this handling) + payload_string += "\n - " + attr + ": " + str([hex(item) for item in getattr(self, attr)]) + + else: # handling of static length messages and text messages + for attr in payload_dict[self.message_id]["field_names"]: + payload_string += "\n - " + attr + ": " + str(getattr(self, attr)) + + representation = ( + "\n\n--------------------------------------------------\n" + "ID: " + str(self.message_id) + " - " + self.name + "\n" + + header_string + "\n" + + payload_string + "\n" + + "Checksum: " + str(self.checksum) + " check: " + str(self.calculate_checksum()) + " pass: " + str(self.verify_checksum()) + ) + + return representation + + +# A class to digest a serial stream and decode PingMessages +class PingParser(object): + NEW_MESSAGE = 0 # Just got a complete checksum-verified message + WAIT_START = 1 # Waiting for the first character of a message 'B' + WAIT_HEADER = 2 # Waiting for the second character in the two-character sequence 'BR' + WAIT_LENGTH_L = 3 # Waiting for the low byte of the payload length field + WAIT_LENGTH_H = 4 # Waiting for the high byte of the payload length field + WAIT_MSG_ID_L = 5 # Waiting for the low byte of the payload id field + WAIT_MSG_ID_H = 6 # Waiting for the high byte of the payload id field + WAIT_SRC_ID = 7 # Waiting for the source device id + WAIT_DST_ID = 8 # Waiting for the destination device id + WAIT_PAYLOAD = 9 # Waiting for the last byte of the payload to come in + WAIT_CHECKSUM_L = 10 # Waiting for the checksum low byte + WAIT_CHECKSUM_H = 11 # Waiting for the checksum high byte + + def __init__(self): + self.buf = bytearray() + self.state = PingParser.WAIT_START + self.payload_length = 0 # payload length remaining to be parsed for the message currently being parsed + self.message_id = 0 # message id of the message currently being parsed + self.errors = 0 + self.parsed = 0 + self.rx_msg = None # most recently parsed message + + # Feed the parser a single byte + # Returns the current parse state + # If the byte fed completes a valid message, return PingParser.NEW_MESSAGE + # The decoded message will be available in the self.rx_msg attribute until a new message is decoded + def parse_byte(self, msg_byte): + if type(msg_byte) != int: + msg_byte = ord(msg_byte) + # print("byte: %d, state: %d, rem: %d, id: %d" % (msg_byte, self.state, self.payload_length, self.message_id)) + if self.state == PingParser.WAIT_START: + self.buf = bytearray() + if msg_byte == ord('B'): + self.buf.append(msg_byte) + self.state += 1 + elif self.state == PingParser.WAIT_HEADER: + if msg_byte == ord('R'): + self.buf.append(msg_byte) + self.state += 1 + else: + self.state = PingParser.WAIT_START + elif self.state == PingParser.WAIT_LENGTH_L: + self.payload_length = msg_byte + self.buf.append(msg_byte) + self.state += 1 + elif self.state == PingParser.WAIT_LENGTH_H: + self.payload_length = (msg_byte << 8) | self.payload_length + self.buf.append(msg_byte) + self.state += 1 + elif self.state == PingParser.WAIT_MSG_ID_L: + self.message_id = msg_byte + self.buf.append(msg_byte) + self.state += 1 + elif self.state == PingParser.WAIT_MSG_ID_H: + self.message_id = (msg_byte << 8) | self.message_id + self.buf.append(msg_byte) + self.state += 1 + elif self.state == PingParser.WAIT_SRC_ID: + self.buf.append(msg_byte) + self.state += 1 + elif self.state == PingParser.WAIT_DST_ID: + self.buf.append(msg_byte) + self.state += 1 + if self.payload_length == 0: # no payload bytes + self.state += 1 + elif self.state == PingParser.WAIT_PAYLOAD: + self.buf.append(msg_byte) + self.payload_length -= 1 + if self.payload_length == 0: + self.state += 1 + elif self.state == PingParser.WAIT_CHECKSUM_L: + self.buf.append(msg_byte) + self.state += 1 + elif self.state == PingParser.WAIT_CHECKSUM_H: + self.buf.append(msg_byte) + self.rx_msg = PingMessage(msg_data=self.buf) + + # print(self.rx_msg) + + self.state = PingParser.WAIT_START + self.payload_length = 0 + self.message_id = 0 + + if self.rx_msg.verify_checksum(): + self.parsed += 1 + return PingParser.NEW_MESSAGE + else: + # TODO add/return error state + print("parse error") + self.errors += 1 + + return self.state + + +if __name__ == "__main__": + # Hand-written data buffers for testing and verification + test_protocol_version_buf = bytearray([ + 0x42, + 0x52, + 4, + 0, + definitions.COMMON_PROTOCOL_VERSION, + 0, + 77, + 211, + 1, + 2, + 3, + 99, + 0x26, + 0x02]) + + test_profile_buf = bytearray([ + 0x42, # 'B' + 0x52, # 'R' + 0x24, # 36_L payload length + 0x00, # 36_H + 0x14, # 1300_L message id + 0x05, # 1300_H + 56, + 45, + 0xe8, # 1000_L distance + 0x03, # 1000_H + 0x00, # 1000_H + 0x00, # 1000_H + 93, # 93_L confidence + 0x00, # 93_H + 0x3f, # 2111_L transmit duration + 0x08, # 2111_H + 0x1c, # 44444444_L ping number + 0x2b, # 44444444_H + 0xa6, # 44444444_H + 0x02, # 44444444_H + 0xa0, # 4000_L scan start + 0x0f, # 4000_H + 0x00, # 4000_H + 0x00, # 4000_H + 0xb8, # 35000_L scan length + 0x88, # 35000_H + 0x00, # 35000_H + 0x00, # 35000_H + 0x04, # 4_L gain setting + 0x00, # 4_H + 0x00, # 4_H + 0x00, # 4_H + 10, # 10_L profile data length + 0x00, # 10_H + 0,1,2,3,4,5,6,7,8,9, # profile data + 0xde, # 1502_H checksum + 0x05 # 1502_L + ]) + + p = PingParser() + + result = None + # A text message + print("\n---Testing protocol_version---\n") + for byte in test_protocol_version_buf: + result = p.parse_byte(byte) + + if result is p.NEW_MESSAGE: + print(p.rx_msg) + else: + print("fail:", result) + exit(1) + + # A dynamic vector message + print("\n---Testing profile---\n") + for byte in test_profile_buf: + result = p.parse_byte(byte) + + if result is p.NEW_MESSAGE: + print(p.rx_msg) + else: + print("fail:", result) + exit(1) diff --git a/src/ping360/polarplot.py b/src/ping360/polarplot.py index 52291b5..205c509 100644 --- a/src/ping360/polarplot.py +++ b/src/ping360/polarplot.py @@ -1,5 +1,70 @@ -#!/usr/bin/env python +import roslib +roslib.load_manifest('ros-ping360-sonar') +from sensor import Ping360 +import argparse +import cv2 +import numpy as np +from math import pi, cos, sin +from std_msgs.msg import String +from sensor_msgs.msg import Image +from cv_bridge import CvBridge, CvBridgeError +import rospy + + def main(): - print "Now executing main() in foo.py" - # Call the bulk of our code... \ No newline at end of file + parser = argparse.ArgumentParser(description="Ping python library example.") + parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.") + parser.add_argument('--baudrate', action="store", type=int, default=2000000, help="Ping device baudrate.") + parser.add_argument('--v', action="store", type=bool, default=False, help="Verbose") + parser.add_argument('--size', action="store", type=int, default=200, help="Length") + + args = parser.parse_args() + try: + rospy.init_node('ping360_node') + p = Ping360(args.device, args.baudrate) + bridge = CvBridge() + imagePub = rospy.Publisher('ping360_sonar', Image, queue_size=1) + print("Initialized: %s" % p.initialize()) + if args.v: + print('Verbose Mode') + print(p.set_transmit_frequency(1000)) + print(p.set_sample_period(80)) + print(p.set_number_of_samples(200)) + + max_range = 80*200*1450/2 + step = 1 + length = args.size + image = np.zeros((length, length, 1), np.uint8) + angle = 0 + center = (length/2,length/2) + while not rospy.is_shutdown(): + p.transmitAngle(angle) + data = bytearray(getattr(p,'_data')) + data_lst = [k for k in data] + linear_factor = len(data_lst)/center[0] + for i in range(int(center[0])): + if(i < center[0]*max_range/max_range): + pointColor = data_lst[int(i*linear_factor-1)] + else: + pointColor = 0 + for k in np.linspace(0,step,8*step): + image[int(center[0]+i*cos(2*pi*(angle+k)/400)), int(center[1]+i*sin(2*pi*(angle+k)/400)), 0] = pointColor + angle = (angle + step) % 400 + color = cv2.applyColorMap(image,cv2.COLORMAP_JET) + image = color + if args.v: + cv2.imshow("PolarPlot",image) + cv2.waitKey(27) + publish(image, imagePub, bridge) + rospy.sleep(0.1) + + except KeyboardInterrupt: + print("Shutting down") + cv2.destroyAllWindows() + +def publish(image, imagePub, bridge): + try: + imagePub.publish(bridge.cv2_to_imgmsg(image, "bgr8")) + except CvBridgeError as e: + print(e) \ No newline at end of file diff --git a/src/ping360/sensor.py b/src/ping360/sensor.py new file mode 100644 index 0000000..2564781 --- /dev/null +++ b/src/ping360/sensor.py @@ -0,0 +1,219 @@ +#!/usr/bin/env python3 + +# ping360.py +# A device API for the Blue Robotics Ping360 scanning sonar + +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 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, 4) is None: + print("empty request") + 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_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], 0.5) + + def transmit(self): + return self.transmitAngle(self._angle) + + From b3c58b71a16a24ab63391f376e7f624565ac17df Mon Sep 17 00:00:00 2001 From: Henrique Martinez Date: Thu, 5 Dec 2019 16:44:30 +0100 Subject: [PATCH 2/7] works on python2 --- find_usb_port.sh | 27 +++++++++++++++++++++++++++ nodes/ping360_node | 3 +++ src/ping360/__init__.py | 4 +++- src/ping360/definitions.py | 2 ++ src/ping360/device.py | 2 +- src/ping360/pingmessage.py | 2 +- src/ping360/polarplot.py | 19 +++++++++++++------ src/ping360/polarplot.pyc | Bin 350 -> 3316 bytes src/ping360/sensor.py | 2 +- 9 files changed, 51 insertions(+), 10 deletions(-) create mode 100755 find_usb_port.sh diff --git a/find_usb_port.sh b/find_usb_port.sh new file mode 100755 index 0000000..9885474 --- /dev/null +++ b/find_usb_port.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# for sysdevpath in $(find /sys/bus/usb/devices/usb*/ -name dev); do +# ( +# syspath="${sysdevpath%/dev}" +# devname="$(udevadm info -q name -p $syspath)" +# [[ "$devname" == "bus/"* ]] && continue +# eval "$(udevadm info -q property --export -p $syspath)" +# [[ -z "$ID_SERIAL" ]] && continue +# echo "/dev/$devname - $ID_SERIAL" +# ) +# done + +for sysdevpath in $(find /sys/bus/usb/devices/usb*/ -name dev); do + ( + syspath="${sysdevpath%/dev}" + devname="$(udevadm info -q name -p $syspath)" + [[ "$devname" == "bus/"* ]] && continue + eval "$(udevadm info -q property --export -p $syspath)" + [[ -z "$ID_SERIAL" ]] && continue + # echo "$ID_SERIAL" + if [ $ID_SERIAL == "FTDI_FT230X_Basic_UART_DO01F2RQ" ]; then + echo "$devname" + fi + ) +done + diff --git a/nodes/ping360_node b/nodes/ping360_node index 2ae4557..84cc5a0 100755 --- a/nodes/ping360_node +++ b/nodes/ping360_node @@ -1,5 +1,8 @@ #!/usr/bin/env python + from ping360 import main + import sys + if __name__ == '__main__': main() \ No newline at end of file diff --git a/src/ping360/__init__.py b/src/ping360/__init__.py index 0dd58c4..ef47465 100644 --- a/src/ping360/__init__.py +++ b/src/ping360/__init__.py @@ -1,4 +1,6 @@ -from . import polarplot.main as main +#!/usr/bin/env python + +from polarplot import main from brping.definitions import * from brping.pingmessage import * from brping.device import PingDevice diff --git a/src/ping360/definitions.py b/src/ping360/definitions.py index c7585eb..084541c 100644 --- a/src/ping360/definitions.py +++ b/src/ping360/definitions.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + COMMON_ACK = 1 COMMON_NACK = 2 COMMON_ASCII_TEXT = 3 diff --git a/src/ping360/device.py b/src/ping360/device.py index 0f7632b..baa0b84 100644 --- a/src/ping360/device.py +++ b/src/ping360/device.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python # device.py # A device API for devices implementing Blue Robotics ping-protocol diff --git a/src/ping360/pingmessage.py b/src/ping360/pingmessage.py index a6ce601..4b8b5a3 100644 --- a/src/ping360/pingmessage.py +++ b/src/ping360/pingmessage.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python # PingMessage.py # Python implementation of the Blue Robotics 'Ping' binary message protocol diff --git a/src/ping360/polarplot.py b/src/ping360/polarplot.py index 205c509..f300a0f 100644 --- a/src/ping360/polarplot.py +++ b/src/ping360/polarplot.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + import roslib roslib.load_manifest('ros-ping360-sonar') @@ -37,22 +39,27 @@ def main(): length = args.size image = np.zeros((length, length, 1), np.uint8) angle = 0 - center = (length/2,length/2) + center = (float(length/2),float(length/2)) while not rospy.is_shutdown(): p.transmitAngle(angle) data = bytearray(getattr(p,'_data')) data_lst = [k for k in data] - linear_factor = len(data_lst)/center[0] + linear_factor = float(len(data_lst))/float(center[0]) for i in range(int(center[0])): if(i < center[0]*max_range/max_range): pointColor = data_lst[int(i*linear_factor-1)] else: pointColor = 0 for k in np.linspace(0,step,8*step): - image[int(center[0]+i*cos(2*pi*(angle+k)/400)), int(center[1]+i*sin(2*pi*(angle+k)/400)), 0] = pointColor + theta = 2*pi*(angle+k)/400.0 + x = float(i)*cos(theta) + y = float(i)*sin(theta) + #print(x,y,theta) + image[int(center[0]+x)][int(center[1]+y)][0] = pointColor + angle = (angle + step) % 400 - color = cv2.applyColorMap(image,cv2.COLORMAP_JET) - image = color + #color = cv2.applyColorMap(image,cv2.COLORMAP_JET) + #image = color if args.v: cv2.imshow("PolarPlot",image) cv2.waitKey(27) @@ -65,6 +72,6 @@ def main(): def publish(image, imagePub, bridge): try: - imagePub.publish(bridge.cv2_to_imgmsg(image, "bgr8")) + imagePub.publish(bridge.cv2_to_imgmsg(image, "mono8")) except CvBridgeError as e: print(e) \ No newline at end of file diff --git a/src/ping360/polarplot.pyc b/src/ping360/polarplot.pyc index 332a2bca0ef2643cd9e66be32acbaf22caf57c68..985a024692ff302ba84b08b668e95d8922169260 100644 GIT binary patch literal 3316 zcmcgu-Ew2a5$-v%Wy_ZQ|I4ly0c%i5Y^Rb6*-CA}E?xp#fa8)=n5eQ*9qEjv@t!}= zjJy^lRZ+D(KynEexyU6|d4ODUm-}4s3Z%H?0pROC_O9UpkaR}VKhx9wbx+SJeOH^= zzWx74UAp`P_`iq8eT5;yPf<+NBGN>silTs?DpD*cQ5;ZGqE@M(p*TUUiK|7E)SA4S zm#H-+^9hQlDXCDaLdguZW+<7Z)@-qUlHw{QHEPugx=isLCG*spFX$ZK z)LMGB>oT>LDVnCw;O7cO6`8EE=SPhhL}9+i=fEvuYBRT!o3!`aH-5e2vNW^}1+JgM za9FS#C1x-Rx$!*jW={T{NeksNm`C11y`U#SKTN`&ZtyBUjbZ=%ZEK>Q_O)xoJJx1a za9#nI9oxs_o?sJSU|YaTYD_c&Ct!>t016z-1Sg^?@*t-<#;0mRT`J97DM1X?S&FKn zRJ*P;cTK6xN(h=Q&0kkqxTaKCS`?+FXG+ublxVDI_|L#s=oDKoLq^Ka(n!&1RYq(a z&^RDJC)y=q)p?3m$YTd&W(^8zj8N8T6wv7+tF68J9$Q!M4G%kx9m;jM=I^Cer6#pPPfivL1B0NO+TQshae?g>J z4_iMXPM|eI7yniq!3}oP9hBIQn>6AaAn+~nx7g9>7TW}CJWD;eK1&ZCRcQ=Fs`R&j z&c0UT8XNa7(ugy|Ki&%vZqX)))$tr%d`rI0S$xihDN%6RD-_x3aHGL05!G=0tQ(Gsn*7nhN&(dKBm;J}5uXT#w5tR5Wm zhGklhUZOQ*;WGQXCBIFluTu0fn*^kaX{7fR_C=v8e1j#U2^y6MK}c?XLD6lIpZ!Z6 zAE$Mi@UPS38M3ydl!~@dB5dZDoI;f<`WPMTsL{XyJV5E9n`zgzCif;w-A&Nk4eI&O z_p`Jfn~n|bu&yt{B#-s3<0HPa6X|o))t=iy*yV)?7WLsXtECB8eKs&wM>w+Zd6;XN z^mUv|E0P@w8}&T1e%EnJ*xBiXgUE(nOH+#UNjQkT`Si0}eKhb;Xdj=-HREfgb3#Sl}+{<}o}> zjW=NotI;3o+s?76%N^?hpE@r4stJCoLz6T;r%#1 zHYr?p4ZdK-x@67@5RyK2Bo|4ixS-ih!t~7H>+94SRL0(etSfcx_TJe)*gLe@srL1wKONj}?jhHE&UX3#$L~GD zy*%=JIiI{7CvO*Z%`qgQNu}6$H&RP#yZ8oGRZ=yzsw!$j&10NabNmfvR2kGQjCJr? zV-vG2Xx#{yx5nS#jbN@+Q48{24Qgsqb-5mRM-F=*kGqRuh-(V30>%BW_+1F^a>eDU zj?)eF_CR|3@A0aH{*P+_ZwpsFoy+V%U`(}hA1F|ks(-v2`GZ1&KabR6Qquj!QBI> z7^23nSPm%WmtU@sT9KMuT9TQUu8^CUnWv$tkeR2DmY=UzP^ke_!GJ~-I|0S?GxBp& z^@~gLi*hq7^pg`yvNQAI%Zv4ki<0$=^YapmbPIsy8Jih^MPTfL{G7z1f}H#kpwT5P pKx07mF$3vhkfRwG{WL)S3IefdWGM&GXq(*pl+v73JFvwdy8!sKPO|_2 diff --git a/src/ping360/sensor.py b/src/ping360/sensor.py index 2564781..59b7d5f 100644 --- a/src/ping360/sensor.py +++ b/src/ping360/sensor.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python # ping360.py # A device API for the Blue Robotics Ping360 scanning sonar From 7efd2885487d696adf9b8d296e75a7cb2130a687 Mon Sep 17 00:00:00 2001 From: Stormiix Date: Thu, 5 Dec 2019 19:23:25 +0100 Subject: [PATCH 3/7] Cleanup and pkg name update --- CMakeLists.txt | 2 +- README.md | 2 +- find_usb_port.sh | 11 ------ nodes/ping360_node | 4 +-- package.xml | 2 +- src/ping360/polarplot.pyc | Bin 3316 -> 0 bytes src/{ping360 => ping360_sonar}/__init__.py | 3 -- src/{ping360 => ping360_sonar}/definitions.py | 0 src/{ping360 => ping360_sonar}/device.py | 5 --- src/{ping360 => ping360_sonar}/pingmessage.py | 0 src/{ping360 => ping360_sonar}/polarplot.py | 33 ++++++++++-------- src/{ping360 => ping360_sonar}/sensor.py | 0 12 files changed, 22 insertions(+), 40 deletions(-) delete mode 100644 src/ping360/polarplot.pyc rename src/{ping360 => ping360_sonar}/__init__.py (74%) rename src/{ping360 => ping360_sonar}/definitions.py (100%) rename src/{ping360 => ping360_sonar}/device.py (98%) rename src/{ping360 => ping360_sonar}/pingmessage.py (100%) rename src/{ping360 => ping360_sonar}/polarplot.py (80%) rename src/{ping360 => ping360_sonar}/sensor.py (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 66af83f..28401e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(ros-ping360-sonar) +project(ping360_sonar) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) diff --git a/README.md b/README.md index 8502e58..4dccb9f 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,2 @@ -# ros-ping360-sonar +# ping360_sonar ROS package for Blue Robotics Ping360 Sonar diff --git a/find_usb_port.sh b/find_usb_port.sh index 9885474..0126705 100755 --- a/find_usb_port.sh +++ b/find_usb_port.sh @@ -1,16 +1,5 @@ #!/bin/bash -# for sysdevpath in $(find /sys/bus/usb/devices/usb*/ -name dev); do -# ( -# syspath="${sysdevpath%/dev}" -# devname="$(udevadm info -q name -p $syspath)" -# [[ "$devname" == "bus/"* ]] && continue -# eval "$(udevadm info -q property --export -p $syspath)" -# [[ -z "$ID_SERIAL" ]] && continue -# echo "/dev/$devname - $ID_SERIAL" -# ) -# done - for sysdevpath in $(find /sys/bus/usb/devices/usb*/ -name dev); do ( syspath="${sysdevpath%/dev}" diff --git a/nodes/ping360_node b/nodes/ping360_node index 84cc5a0..a4cb473 100755 --- a/nodes/ping360_node +++ b/nodes/ping360_node @@ -1,8 +1,6 @@ #!/usr/bin/env python -from ping360 import main - -import sys +from ping360_sonar import main if __name__ == '__main__': main() \ No newline at end of file diff --git a/package.xml b/package.xml index 32b2647..ad646c2 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ - ros-ping360-sonar + ping360_sonar 0.0.0 The sonar-ping360 package diff --git a/src/ping360/polarplot.pyc b/src/ping360/polarplot.pyc deleted file mode 100644 index 985a024692ff302ba84b08b668e95d8922169260..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3316 zcmcgu-Ew2a5$-v%Wy_ZQ|I4ly0c%i5Y^Rb6*-CA}E?xp#fa8)=n5eQ*9qEjv@t!}= zjJy^lRZ+D(KynEexyU6|d4ODUm-}4s3Z%H?0pROC_O9UpkaR}VKhx9wbx+SJeOH^= zzWx74UAp`P_`iq8eT5;yPf<+NBGN>silTs?DpD*cQ5;ZGqE@M(p*TUUiK|7E)SA4S zm#H-+^9hQlDXCDaLdguZW+<7Z)@-qUlHw{QHEPugx=isLCG*spFX$ZK z)LMGB>oT>LDVnCw;O7cO6`8EE=SPhhL}9+i=fEvuYBRT!o3!`aH-5e2vNW^}1+JgM za9FS#C1x-Rx$!*jW={T{NeksNm`C11y`U#SKTN`&ZtyBUjbZ=%ZEK>Q_O)xoJJx1a za9#nI9oxs_o?sJSU|YaTYD_c&Ct!>t016z-1Sg^?@*t-<#;0mRT`J97DM1X?S&FKn zRJ*P;cTK6xN(h=Q&0kkqxTaKCS`?+FXG+ublxVDI_|L#s=oDKoLq^Ka(n!&1RYq(a z&^RDJC)y=q)p?3m$YTd&W(^8zj8N8T6wv7+tF68J9$Q!M4G%kx9m;jM=I^Cer6#pPPfivL1B0NO+TQshae?g>J z4_iMXPM|eI7yniq!3}oP9hBIQn>6AaAn+~nx7g9>7TW}CJWD;eK1&ZCRcQ=Fs`R&j z&c0UT8XNa7(ugy|Ki&%vZqX)))$tr%d`rI0S$xihDN%6RD-_x3aHGL05!G=0tQ(Gsn*7nhN&(dKBm;J}5uXT#w5tR5Wm zhGklhUZOQ*;WGQXCBIFluTu0fn*^kaX{7fR_C=v8e1j#U2^y6MK}c?XLD6lIpZ!Z6 zAE$Mi@UPS38M3ydl!~@dB5dZDoI;f<`WPMTsL{XyJV5E9n`zgzCif;w-A&Nk4eI&O z_p`Jfn~n|bu&yt{B#-s3<0HPa6X|o))t=iy*yV)?7WLsXtECB8eKs&wM>w+Zd6;XN z^mUv|E0P@w8}&T1e%EnJ*xBiXgUE(nOH+#UNjQkT`Si0}eKhb;Xdj=-HREfgb3#Sl}+{<}o}> zjW=NotI;3o+s?76%N^?hpE@r4stJCoLz6T;r%#1 zHYr?p4ZdK-x@67@5RyK2Bo|4ixS-ih!t~7H>+94SRL0(etSfcx_TJe)*gLe@srL1wKONj}?jhHE&UX3#$L~GD zy*%=JIiI{7CvO*Z%`qgQNu}6$H&RP#yZ8oGRZ=yzsw!$j&10NabNmfvR2kGQjCJr? zV-vG2Xx#{yx5nS#jbN@+Q48{24Qgsqb-5mRM-F=*kGqRuh-(V30>%BW_+1F^a>eDU zj?)eF_CR|3@A0aH{*P+_ZwpsFoy+V%U`( Date: Thu, 5 Dec 2019 20:48:04 +0100 Subject: [PATCH 4/7] cleanup and correct params values --- src/ping360_sonar/polarplot.py | 83 ++++++++++++++++++++++++++++------ 1 file changed, 68 insertions(+), 15 deletions(-) diff --git a/src/ping360_sonar/polarplot.py b/src/ping360_sonar/polarplot.py index 3f9e090..cf71ef3 100644 --- a/src/ping360_sonar/polarplot.py +++ b/src/ping360_sonar/polarplot.py @@ -13,7 +13,6 @@ from cv_bridge import CvBridge, CvBridgeError import rospy - def main(): parser = argparse.ArgumentParser(description="Ping python library example.") parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.") @@ -24,35 +23,50 @@ def main(): args = parser.parse_args() try: rospy.init_node('ping360_node') - transmitFrequency = 1000 - samplePeriod = 80 - numberOfSample = 200 - speedOfSound = 1450 - length = args.size + + # Ping 360 Parameters + device = "/dev/ttyUSB0" + baudrate = 115200 + gain = 0 + numberOfSamples = 1200 # Numbre of points + transmitFrequency = 740 # Default frequency + sonarRange = 30 # in m + speedOfSound = 1500 # in km/h ? + samplePeriod = calculateSamplePeriod(sonarRange, numberOfSamples, speedOfSound) + transmitDuration = adjustTransmitDuration(sonarRange, samplePeriod, speedOfSound) + + # Output and ROS parameters step = 1 - angle = 0 topic = "ping360_sonar" + imgSize = args.size queue_size= 1 + # Global Variables + angle = 0 p = Ping360(args.device, args.baudrate) imagePub = rospy.Publisher(topic, Image, queue_size=queue_size) bridge = CvBridge() + # Initialize and configure the sonar print("Initialized: %s" % p.initialize()) + p.set_gain_setting(gain) p.set_transmit_frequency(transmitFrequency) + p.set_transmit_duration(transmitDuration) p.set_sample_period(samplePeriod) - p.set_number_of_samples(numberOfSample) + p.set_number_of_samples(numberOfSamples) + + # Create a new mono-channel image + image = np.zeros((imgSize, imgSize, 1), np.uint8) - max_range = samplePeriod * numberOfSample * speedOfSound / 2 - image = np.zeros((length, length, 1), np.uint8) - center = (float(length/2),float(length/2)) + # Center point coordinates + center = (float(imgSize/2),float(imgSize/2)) while not rospy.is_shutdown(): p.transmitAngle(angle) data = bytearray(getattr(p,'_data')) data_lst = [k for k in data] - linear_factor = float(len(data_lst))/float(center[0]) - for i in range(int(center[0])): - if(i < center[0]*max_range/max_range): + linear_factor = float(len(data_lst)) / float(center[0]) #TODO: this should probably be range/pixelsize + for i in range(int(center[0])): #TODO: check the update polar logic on ping-viewer + if(i < center[0]): pointColor = data_lst[int(i*linear_factor-1)] else: pointColor = 0 @@ -77,4 +91,43 @@ def publish(image, imagePub, bridge): try: imagePub.publish(bridge.cv2_to_imgmsg(image, "mono8")) except CvBridgeError as e: - print(e) \ No newline at end of file + print(e) + +# https://discuss.bluerobotics.com/t/please-provide-some-answer-regards-ping360/6393/3?u=stormix + +def calculateSamplePeriod(distance: int, numberOfSamples: int, speedOfSound: int, _samplePeriodTickDuration = 25e-9) -> float: + """ + Calculate the sample period based in the new range + """ + return 2*distance/(numberOfSamples*speedOfSound*_samplePeriodTickDuration) + +def adjustTransmitDuration(distance: int, samplePeriod: float, speedOfSound: int, _firmwareMinTransmitDuration = 5) -> float: + """ + @brief Adjust the transmit duration for a specific range + Per firmware engineer: + 1. Starting point is TxPulse in usec = ((one-way range in metres) * 8000) / (Velocity of sound in metres + per second) + 2. Then check that TxPulse is wide enough for currently selected sample interval in usec, i.e., + if TxPulse < (2.5 * sample interval) then TxPulse = (2.5 * sample interval) + 3. Perform limit checking + @return Transmit duration + """ + # 1 + duration = 8000 * distance / speedOfSound + # 2 (transmit duration is microseconds, samplePeriod() is nanoseconds) + transmit_duration = max(2.5*samplePeriod()/1000, duration) + # 3 + return max(_firmwareMinTransmitDuration, min(transmitDurationMax(samplePeriod), transmit_duration)) + +def transmitDurationMax(samplePeriod: float, _firmwareMaxTransmitDuration = 500) -> float: + """ + @brief The maximum transmit duration that will be applied is limited internally by the + firmware to prevent damage to the hardware + The maximum transmit duration is equal to 64 * the sample period in microseconds + @return The maximum transmit duration possible + """ + return min(_firmwareMaxTransmitDuration, samplePeriod(samplePeriod) * 64e6) + +def samplePeriod(samplePeriod: float, _samplePeriodTickDuration = 25e-9) -> float: + """ Sample period in ns """ + return samplePeriod*_samplePeriodTickDuration \ No newline at end of file From b962bcfa21eabe9b724d0a76a839d08952711bfa Mon Sep 17 00:00:00 2001 From: Henrique Martinez Date: Fri, 6 Dec 2019 09:51:01 +0100 Subject: [PATCH 5/7] [FIX] python 2.7 compatibility --- setup.py | 2 +- src/ping360_sonar/polarplot.py | 22 +++++++++++++--------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/setup.py b/setup.py index 2177cd9..f5b4f45 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ from catkin_pkg.python_setup import generate_distutils_setup setup_args = generate_distutils_setup( - packages=['ping360'], + packages=['ping360_sonar'], package_dir={'': 'src'}, install_requires=['bluerobotics-ping'] ) diff --git a/src/ping360_sonar/polarplot.py b/src/ping360_sonar/polarplot.py index cf71ef3..704ff4e 100644 --- a/src/ping360_sonar/polarplot.py +++ b/src/ping360_sonar/polarplot.py @@ -28,10 +28,10 @@ def main(): device = "/dev/ttyUSB0" baudrate = 115200 gain = 0 - numberOfSamples = 1200 # Numbre of points + numberOfSamples = 200 # Number of points transmitFrequency = 740 # Default frequency - sonarRange = 30 # in m - speedOfSound = 1500 # in km/h ? + sonarRange = 1 # in m + speedOfSound = 1500 # in m/s samplePeriod = calculateSamplePeriod(sonarRange, numberOfSamples, speedOfSound) transmitDuration = adjustTransmitDuration(sonarRange, samplePeriod, speedOfSound) @@ -95,13 +95,15 @@ def publish(image, imagePub, bridge): # https://discuss.bluerobotics.com/t/please-provide-some-answer-regards-ping360/6393/3?u=stormix -def calculateSamplePeriod(distance: int, numberOfSamples: int, speedOfSound: int, _samplePeriodTickDuration = 25e-9) -> float: +def calculateSamplePeriod(distance, numberOfSamples, speedOfSound, _samplePeriodTickDuration = 25e-9): + # type: (float, int, int, float) -> float """ Calculate the sample period based in the new range """ return 2*distance/(numberOfSamples*speedOfSound*_samplePeriodTickDuration) -def adjustTransmitDuration(distance: int, samplePeriod: float, speedOfSound: int, _firmwareMinTransmitDuration = 5) -> float: +def adjustTransmitDuration(distance, samplePeriod, speedOfSound, _firmwareMinTransmitDuration = 5): + # type: (float, float, int, int) -> float """ @brief Adjust the transmit duration for a specific range Per firmware engineer: @@ -115,19 +117,21 @@ def adjustTransmitDuration(distance: int, samplePeriod: float, speedOfSound: int # 1 duration = 8000 * distance / speedOfSound # 2 (transmit duration is microseconds, samplePeriod() is nanoseconds) - transmit_duration = max(2.5*samplePeriod()/1000, duration) + transmit_duration = max(2.5*getSamplePeriod(samplePeriod)/1000, duration) # 3 return max(_firmwareMinTransmitDuration, min(transmitDurationMax(samplePeriod), transmit_duration)) -def transmitDurationMax(samplePeriod: float, _firmwareMaxTransmitDuration = 500) -> float: +def transmitDurationMax(samplePeriod, _firmwareMaxTransmitDuration = 500): + # type: (float, int) -> float """ @brief The maximum transmit duration that will be applied is limited internally by the firmware to prevent damage to the hardware The maximum transmit duration is equal to 64 * the sample period in microseconds @return The maximum transmit duration possible """ - return min(_firmwareMaxTransmitDuration, samplePeriod(samplePeriod) * 64e6) + return min(_firmwareMaxTransmitDuration, getSamplePeriod(samplePeriod) * 64e6) -def samplePeriod(samplePeriod: float, _samplePeriodTickDuration = 25e-9) -> float: +def getSamplePeriod(samplePeriod, _samplePeriodTickDuration = 25e-9): + # type: (float, float) -> float """ Sample period in ns """ return samplePeriod*_samplePeriodTickDuration \ No newline at end of file From 4592ef47b7b0688c2b0e1db773d718ded5711c6b Mon Sep 17 00:00:00 2001 From: Henrique Martinez Date: Fri, 6 Dec 2019 09:55:15 +0100 Subject: [PATCH 6/7] [FIX] index out of range --- src/ping360_sonar/polarplot.py | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/ping360_sonar/polarplot.py b/src/ping360_sonar/polarplot.py index 704ff4e..a58d78e 100644 --- a/src/ping360_sonar/polarplot.py +++ b/src/ping360_sonar/polarplot.py @@ -65,17 +65,20 @@ def main(): data = bytearray(getattr(p,'_data')) data_lst = [k for k in data] linear_factor = float(len(data_lst)) / float(center[0]) #TODO: this should probably be range/pixelsize - for i in range(int(center[0])): #TODO: check the update polar logic on ping-viewer - if(i < center[0]): - pointColor = data_lst[int(i*linear_factor-1)] - else: - pointColor = 0 - for k in np.linspace(0,step,8*step): - theta = 2*pi*(angle+k)/400.0 - x = float(i)*cos(theta) - y = float(i)*sin(theta) - image[int(center[0]+x)][int(center[1]+y)][0] = pointColor - + try: + for i in range(int(center[0])): #TODO: check the update polar logic on ping-viewer + if(i < center[0]): + pointColor = data_lst[int(i*linear_factor-1)] + else: + pointColor = 0 + for k in np.linspace(0,step,8*step): + theta = 2*pi*(angle+k)/400.0 + x = float(i)*cos(theta) + y = float(i)*sin(theta) + image[int(center[0]+x)][int(center[1]+y)][0] = pointColor + except IndexError: + continue + angle = (angle + step) % 400 if args.v: cv2.imshow("PolarPlot",image) From e8673a83a17c1df03386b66781cc2b51d7de62cd Mon Sep 17 00:00:00 2001 From: Henrique Martinez Date: Fri, 6 Dec 2019 10:10:01 +0100 Subject: [PATCH 7/7] [ADD] ros parameters --- launch/example.launch | 15 +++++++++++++++ src/ping360_sonar/polarplot.py | 32 +++++++++++++------------------- 2 files changed, 28 insertions(+), 19 deletions(-) create mode 100644 launch/example.launch diff --git a/launch/example.launch b/launch/example.launch new file mode 100644 index 0000000..851581e --- /dev/null +++ b/launch/example.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/ping360_sonar/polarplot.py b/src/ping360_sonar/polarplot.py index a58d78e..e0e65f3 100644 --- a/src/ping360_sonar/polarplot.py +++ b/src/ping360_sonar/polarplot.py @@ -14,36 +14,30 @@ import rospy def main(): - parser = argparse.ArgumentParser(description="Ping python library example.") - parser.add_argument('--device', action="store", required=True, type=str, help="Ping device port.") - parser.add_argument('--baudrate', action="store", type=int, default=2000000, help="Ping device baudrate.") - parser.add_argument('--v', action="store", type=bool, default=False, help="Verbose") - parser.add_argument('--size', action="store", type=int, default=200, help="Image Size") - - args = parser.parse_args() try: rospy.init_node('ping360_node') # Ping 360 Parameters - device = "/dev/ttyUSB0" - baudrate = 115200 - gain = 0 - numberOfSamples = 200 # Number of points - transmitFrequency = 740 # Default frequency - sonarRange = 1 # in m - speedOfSound = 1500 # in m/s + device = rospy.get_param('~device',"/dev/ttyUSB0") + baudrate = rospy.get_param('~baudrate', 115200) + gain = rospy.get_param('~gain', 0) + numberOfSamples = rospy.get_param('~numberOfSamples', 200) # Number of points + transmitFrequency = rospy.get_param('~transmitFrequency', 740) # Default frequency + sonarRange = rospy.get_param('~sonarRange', 1) # in m + speedOfSound = rospy.get_param('~speedOfSound', 1500) # in m/s samplePeriod = calculateSamplePeriod(sonarRange, numberOfSamples, speedOfSound) transmitDuration = adjustTransmitDuration(sonarRange, samplePeriod, speedOfSound) + debug = rospy.get_param('~debug', True) # Output and ROS parameters - step = 1 + step = rospy.get_param('~step', 1) topic = "ping360_sonar" - imgSize = args.size - queue_size= 1 + imgSize = rospy.get_param('~imgSize', 500) + queue_size= rospy.get_param('~queueSize', 1) # Global Variables angle = 0 - p = Ping360(args.device, args.baudrate) + p = Ping360(device, baudrate) imagePub = rospy.Publisher(topic, Image, queue_size=queue_size) bridge = CvBridge() @@ -80,7 +74,7 @@ def main(): continue angle = (angle + step) % 400 - if args.v: + if debug: cv2.imshow("PolarPlot",image) cv2.waitKey(27) publish(image, imagePub, bridge)