diff --git a/CMakeLists.txt b/CMakeLists.txt index 5a24cb2..5bef9f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs dynamic_reconfigure + message_generation ) find_package(OpenCV REQUIRED) @@ -50,11 +51,10 @@ catkin_python_setup() ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + FILES + SonarEcho.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -71,10 +71,10 @@ catkin_python_setup() # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# sensor_msgs# std_msgs -# ) +generate_messages( + DEPENDENCIES + sensor_msgs# std_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## diff --git a/README.md b/README.md index fde15d8..9721aff 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ A python ROS package for the [BlueRobotics] [Ping360] Sonar. The package has been tested under [ROS] melodic and Ubuntu 16.04. This code is mostly experimental, expect that it changes often. -**Keywords:** ROS, package, ping360 +**Keywords:** ROS, package, ping360, ping360 emulator ### License @@ -18,6 +18,10 @@ The source code is released under a [MIT license](LICENSE). ## Installation +### Download the latest release + +Get the latest stable release [here](https://github.com/CentraleNantesRobotics/ping360_sonar_python/releases/latest). + ### Building from Source #### Dependencies @@ -28,6 +32,10 @@ The source code is released under a [MIT license](LICENSE). #### Building +Before building from source, install [ping-protocol Python Lib](https://pypi.org/project/bluerobotics-ping/): + + pip install bluerobotics-ping + To build from source, clone the latest version from this repository into your catkin workspace and compile the package using cd catkin_workspace/src @@ -39,7 +47,6 @@ To build from source, clone the latest version from this repository into your ca TODO - ## Usage An example launch file has been provided [example.launch](launch/example.launch): @@ -52,18 +59,37 @@ Run the main node with: * **example.launch:** contains the default parameteres to run the Ping360 Sonar, including the serial port and the baudrate to interface with the sonar. The rest of the parameters are documented here: [Ping360 Documentation](https://docs.bluerobotics.com/ping-protocol/pingmessage-ping360/). The same parameters can also be reconfigured using the [dynamic_reconfigure](http://wiki.ros.org/dynamic_reconfigure). + An emulated mode was added to test the package when you don't have the sonar. To run the emulated sonar, set the env variable to "true": + + + ## Nodes ### ping360_node -Creates a black and white OpenCv image using the date received from the sonar. Same as the one generated by the ping viewer. +While continuously rotating the sonar, it publishes two types of messages: +- The sonar's response data (the echo intensities for a given angle & range) +- A black and white image using the date received from the sonar. Same as the one generated by the ping viewer. #### Published Topics -* **`/ping360_sonar`** ([sensor_msgs/Image]) +* **`/ping360_node/sonar/images`** ([sensor_msgs/Image]) + + The generated sonar image. + +* **`/ping360_node/sonar/data`** ([msg/SonarEcho]) - The generated sonar image. + Publishes the raw sonar data in a custom message: + + Header header #header info + float32 angle # the measurement angle [rad] + uint8 gain # Sonar Gain + uint16 number_of_samples + uint16 transmit_frequency # [kHz] + uint16 speed_of_sound # [m/s] + uint8 range # range value [m] + uint8[] intensities # intensity data [0-255]. This is the actual data received from the sonar ## Bugs & Feature Requests @@ -76,7 +102,7 @@ Please report bugs and request features using the [Issue Tracker](https://github [cv_bridge]: http://wiki.ros.org/cv_bridge [sensor_msgs/Image]: http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Image.html [Ping360]: https://bluerobotics.com/store/sensors-sonars-cameras/sonar/ping360-sonar-r1-rp/ - +[msg/SonarEcho]: /msg/SonarEcho.msg ## Contributors ✨ @@ -93,5 +119,6 @@ Please report bugs and request features using the [Issue Tracker](https://github -This project follows the [all-contributors](https://github.com/all-contributors/all-contributors) specification. Contributions of any kind welcome! Please refer to our [Contribution Guide](CONTRIBUTING.md) +This project follows the [all-contributors](https://github.com/all-contributors/all-contributors) specification. +Contributions of any kind welcome! Please refer to our [Contribution Guide](CONTRIBUTING.md) diff --git a/TODO b/TODO new file mode 100644 index 0000000..e69de29 diff --git a/launch/example.launch b/launch/example.launch index 851581e..7c091ef 100644 --- a/launch/example.launch +++ b/launch/example.launch @@ -1,4 +1,5 @@ + diff --git a/msg/SonarEcho.msg b/msg/SonarEcho.msg new file mode 100644 index 0000000..179b59e --- /dev/null +++ b/msg/SonarEcho.msg @@ -0,0 +1,8 @@ +Header header #header info +float32 angle # the measurement angle [rad] +uint8 gain # Sonar Gain +uint16 number_of_samples +uint16 transmit_frequency # [kHz] +uint16 speed_of_sound # [m/s] +uint8 range # range value [m] +uint8[] intensities # intensity data [0-255]. This is the actual data received from the sonar \ No newline at end of file diff --git a/package.xml b/package.xml index 76a132c..5919b3e 100644 --- a/package.xml +++ b/package.xml @@ -16,6 +16,7 @@ dynamic_reconfigure sensor_msgs std_msgs + message_generation cv_bridge rospy sensor_msgs @@ -25,4 +26,5 @@ sensor_msgs std_msgs dynamic_reconfigure + message_runtime diff --git a/src/ping360_sonar/Emulator.py b/src/ping360_sonar/Emulator.py new file mode 100644 index 0000000..4db331c --- /dev/null +++ b/src/ping360_sonar/Emulator.py @@ -0,0 +1,187 @@ + +from brping import definitions, PingMessage, PingParser +import time +import errno +import math +import random +import os + +verbose = False +payload_dict = definitions.payload_dict_all +# a Serial class emulator +class Serial: + + ## init(): the constructor. Many of the arguments have default values + # and can be skipped when calling the constructor. + def __init__( self, port='COM1', baudrate = 115200, timeout=1, + bytesize = 8, parity = 'N', stopbits = 1, xonxoff=0, + rtscts = 0): + self.name = port + self.port = port + self.timeout = timeout + self.parity = parity + self.baudrate = baudrate + self.bytesize = bytesize + self.stopbits = stopbits + self.xonxoff = xonxoff + self.rtscts = rtscts + self._isOpen = True + self._receivedData = "" + self.in_waiting = 1 + + self.parser = PingParser() # used to parse incoming client comunications + + self._gain_setting = 0 + self._mode = 0 + self._angle = 0 + self._transmit_duration = 0 + self._sample_period = 0 + self._transmit_frequency = 100 + self._number_of_samples = 10 + self._data = "".join([chr(0) for _ in xrange(self._number_of_samples)]) + self._data_length = 10 + + ## isOpen() + # returns True if the port to the Arduino is open. False otherwise + def isOpen( self ): + return self._isOpen + + def send_break(self): + pass + + ## open() + # opens the port + def open( self ): + self._isOpen = True + + ## close() + # closes the port + def close( self ): + self._isOpen = False + + ## write() + # writes a string of characters to the Arduino + def write( self, data ): + try: + # digest data coming in from client + for byte in data: + if self.parser.parse_byte(byte) == PingParser.NEW_MESSAGE: + # we decoded a message from the client + self.handleMessage(self.parser.rx_msg) + + except EnvironmentError as e: + if e.errno == errno.EAGAIN: + pass # waiting for data + else: + if verbose: + print("Error reading data", e) + + except KeyError as e: + if verbose: + print("skipping unrecognized message id: %d" % self.parser.rx_msg.message_id) + print("contents: %s" % self.parser.rx_msg.msg_data) + pass + + ## read() + # reads n characters from the fake Arduino. Actually n characters + # are read from the string _data and returned to the caller. + def read( self, n=1 ): + s = self._read_data[0:n] + self._read_data = self._read_data[n:] + return s + + ## readline() + # reads characters from the fake Arduino until a \n is found. + def readline( self ): + returnIndex = self._read_data.index( "\n" ) + if returnIndex != -1: + s = self._read_data[0:returnIndex+1] + self._read_data = self._read_data[returnIndex+1:] + return s + else: + return "" + + ## __str__() + # returns a string representation of the serial class + def __str__( self ): + return "Serial( port='%s', baudrate=%d," \ + % ( str(self.isOpen), self.port, self.baudrate ) \ + + " bytesize=%d, parity='%s', stopbits=%d, xonxoff=%d, rtscts=%d)"\ + % ( self.bytesize, self.parity, self.stopbits, self.xonxoff, + self.rtscts ) + + # Send a message to the client, the message fields are populated by the + # attributes of this object (either variable or method) with names matching + # the message field names + def sendMessage(self, message_id): + msg = PingMessage(message_id) + if verbose: + print("sending message %d\t(%s)" % (msg.message_id, msg.name)) + # pull attributes of this class into the message fields (they are named the same) + for attr in payload_dict[message_id]["field_names"]: + try: + # see if we have a function for this attribute (dynamic data) + # if so, call it and put the result in the message field + setattr(msg, attr, getattr(self, attr)()) + except AttributeError as e: + try: + # if we don't have a function for this attribute, check for a _ member + # these are static values (or no function implemented yet) + setattr(msg, attr, getattr(self, "_" + attr)) + except AttributeError as e: + # anything else we haven't implemented yet, just send a sine wave + setattr(msg, attr, self.periodicFnInt(20, 120)) + # send the message to the client + msg.pack_msg_data() + self._read_data = msg.msg_data + + # handle an incoming client message + def handleMessage(self, message): + if verbose: + print("receive message %d\t(%s)" % (message.message_id, message.name)) + if message.message_id == definitions.COMMON_GENERAL_REQUEST: + # the client is requesting a message from us + self.sendMessage(message.requested_id) + # hack for legacy requests + elif message.payload_length == 0: + self.sendMessage(message.message_id) + elif message.message_id == definitions.PING360_TRANSDUCER: + # the client is controlling some parameter of the device + self.setParameters(message) + else: + if verbose: + print("Unknown msg type") + # Extract message fields into attribute values + # This should only be performed with the 'set' category of messages + # TODO: mechanism to filter by "set" + def setParameters(self, message): + for attr in payload_dict[message.message_id]["field_names"]: + setattr(self, "_" + attr, getattr(message, attr)) + self.sendDataResponse(message) + + def sendDataResponse(self, message): + # Send a response + self.generateRandomData() + msg = PingMessage(definitions.PING360_DEVICE_DATA) + if verbose: + print("sending a reply %d\t(%s)" % (msg.message_id, msg.name)) + # pull attributes of this class into the message fields (they are named the same) + for attr in payload_dict[definitions.PING360_DEVICE_DATA]["field_names"]: + setattr(msg, attr, getattr(self, "_" + attr)) + # send the message to the client + msg.pack_msg_data() + self._read_data = msg.msg_data + + def generateRandomData(self): + random.seed() + self._data = "".join([chr(random.randint(0, 255)) for _ in range(self._number_of_samples)]) + + ########### + # Helpers for generating periodic data + ########### + + def periodicFn(self, amplitude = 0, offset = 0, frequency = 1.0, shift = 0): + return amplitude * math.sin(frequency * time.time() + shift) + offset + + def periodicFnInt(self, amplitude = 0, offset = 0, frequency = 1.0, shift = 0): + return int(self.periodicFn(amplitude, offset, frequency, shift)) \ No newline at end of file diff --git a/src/ping360_sonar/__init__.py b/src/ping360_sonar/__init__.py index 77e5266..d6967d7 100644 --- a/src/ping360_sonar/__init__.py +++ b/src/ping360_sonar/__init__.py @@ -1,4 +1,4 @@ -from polarplot import main +from node import main from brping.definitions import * from brping.pingmessage import * from brping.device import PingDevice diff --git a/src/ping360_sonar/definitions.py b/src/ping360_sonar/definitions.py deleted file mode 100644 index 084541c..0000000 --- a/src/ping360_sonar/definitions.py +++ /dev/null @@ -1,446 +0,0 @@ -#!/usr/bin/env python - -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_sonar/device.py b/src/ping360_sonar/device.py index 9ff07f0..3df74e6 100644 --- a/src/ping360_sonar/device.py +++ b/src/ping360_sonar/device.py @@ -6,7 +6,12 @@ from brping import definitions from brping import pingmessage from collections import deque -import serial + +import os +if os.getenv('emulated_sonar') == 'true': + import Emulator as serial +else: + import serial import time class PingDevice(object): diff --git a/src/ping360_sonar/node.py b/src/ping360_sonar/node.py new file mode 100644 index 0000000..b765c40 --- /dev/null +++ b/src/ping360_sonar/node.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python + +import argparse +import json +from math import cos, pi, sin + +import cv2 +import numpy as np +import rospy + +from cv_bridge import CvBridge, CvBridgeError +from dynamic_reconfigure.server import Server +from sensor_msgs.msg import Image +from std_msgs.msg import String + +from ping360_sonar.cfg import sonarConfig +from ping360_sonar.msg import SonarEcho +from sensor import Ping360 + + +def callback(config, level): + global updated, gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, speedOfSound, samplePeriod, debug, step, imgSize, queue_size, p + # Update Ping 360 Parameters + gain = config['gain'] + numberOfSamples = config['numberOfSamples'] + transmitFrequency = config['transmitFrequency'] + sonarRange = config['range'] + speedOfSound = config['speedOfSound'] + samplePeriod = calculateSamplePeriod(sonarRange, numberOfSamples, speedOfSound) + transmitDuration = adjustTransmitDuration(sonarRange, samplePeriod, speedOfSound) + debug = config['debug'] + step = config['step'] + queue_size = config['queueSize'] + rospy.loginfo("Reconfigure Request") + updated = True + return config + +def main(): + rospy.init_node('ping360_node') + srv = Server(sonarConfig, callback) + + # Global Variables + angle = 0 + bridge = CvBridge() + + # Topic publishers + imagePub = rospy.Publisher("/ping360_node/sonar/images", Image, queue_size=queue_size) + rawPub = rospy.Publisher("/ping360_node/sonar/data", SonarEcho, queue_size=queue_size) + + # Initialize and configure the sonar + updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples) + + # Create a new mono-channel image + image = np.zeros((imgSize, imgSize, 1), np.uint8) + + # Center point coordinates + center = (float(imgSize/2),float(imgSize/2)) + + rate = rospy.Rate(100) # 10hz + + while not rospy.is_shutdown(): + + # Update to the latest config data + if updated: + updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples) + + # Get sonar response + data = getSonarData(angle) + + # Contruct and publish Sonar data msg + rawDataMsg = generateRawMsg(angle, data) # TODO: check for empty responses + rawPub.publish(rawDataMsg) + + # Contruct and publish Sonar image msg + linear_factor = float(len(data)) / float(center[0]) #TODO: this should probably be range/pixelsize + try: + for i in range(int(center[0])): #TODO: check the update polar logic on the new ping-viewer + if(i < center[0]): + pointColor = data[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: + rospy.logwarn("IndexError: data response was empty, skipping this iteration..") + continue + + angle = (angle + step) % 400 + if debug: + cv2.imshow("PolarPlot",image) + cv2.waitKey(1) + else: + cv2.destroyAllWindows() + publishImage(image, imagePub, bridge) + rate.sleep() + +def getSonarData(angle): + """ + Transmits the sonar angle and returns the sonar intensities + Params: + angle int (Gradian Angle) + Return: + data list Intensities from 0 - 255 + """ + p.transmitAngle(angle) + data = bytearray(getattr(p,'_data')) + return [k for k in data] + +def generateRawMsg(angle, data): + """ + Generates the raw message for the data topic + Params: + angle int (Gradian Angle) + data list List of intensities + """ + msg = SonarEcho() + msg.header.stamp = rospy.Time.now() + msg.header.frame_id = 'sonar_frame' + msg.angle = angle + msg.gain = gain + msg.number_of_samples = numberOfSamples + msg.transmit_frequency = transmitFrequency + msg.speed_of_sound = speedOfSound + msg.range = sonarRange + msg.intensities = data + return msg + +def publishImage(image, imagePub, bridge): + try: + imagePub.publish(bridge.cv2_to_imgmsg(image, "mono8")) + except CvBridgeError as e: + rospy.logwarn("Failed to publish sensor image") + print(e) + +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, samplePeriod, speedOfSound, _firmwareMinTransmitDuration = 5): + # type: (float, float, int, int) -> 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 * getSamplePeriod(samplePeriod) / 1000, duration) + # 3 + return max(_firmwareMinTransmitDuration, min(transmitDurationMax(samplePeriod), transmit_duration)) + +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, getSamplePeriod(samplePeriod) * 64e6) + +def getSamplePeriod(samplePeriod, _samplePeriodTickDuration = 25e-9): + # type: (float, float) -> float + """ Sample period in ns """ + return samplePeriod * _samplePeriodTickDuration + +def updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples): + global updated + 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(numberOfSamples) + updated = False + +# Ping 360 Parameters +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 = rospy.get_param('~step', 1) +imgSize = rospy.get_param('~imgSize', 500) +queue_size= rospy.get_param('~queueSize', 1) + +p = Ping360(device, baudrate) +updated = False +print("Initialized: %s" % p.initialize()) diff --git a/src/ping360_sonar/pingmessage.py b/src/ping360_sonar/pingmessage.py deleted file mode 100644 index 4b8b5a3..0000000 --- a/src/ping360_sonar/pingmessage.py +++ /dev/null @@ -1,449 +0,0 @@ -#!/usr/bin/env python - -# 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_sonar/polarplot.py b/src/ping360_sonar/polarplot.py deleted file mode 100644 index 381a984..0000000 --- a/src/ping360_sonar/polarplot.py +++ /dev/null @@ -1,164 +0,0 @@ -#!/usr/bin/env python - -import roslib -roslib.load_manifest('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 -from dynamic_reconfigure.server import Server -from ping360_sonar.cfg import sonarConfig -import json - -def callback(config, level): - global gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, speedOfSound, samplePeriod, debug, step, imgSize, queue_size, p - # Update Ping 360 Parameters - gain = config['gain'] - numberOfSamples = config['numberOfSamples'] - transmitFrequency = config['transmitFrequency'] - sonarRange = config['range'] - speedOfSound = config['speedOfSound'] - samplePeriod = calculateSamplePeriod(sonarRange, numberOfSamples, speedOfSound) - transmitDuration = adjustTransmitDuration(sonarRange, samplePeriod, speedOfSound) - debug = config['debug'] - step = config['step'] - queue_size = config['queueSize'] - rospy.loginfo("Reconfigure Request") - return config - - -def main(): - try: - rospy.init_node('ping360_node') - srv = Server(sonarConfig, callback) - - # Global Variables - angle = 0 - imagePub = rospy.Publisher("ping360_sonar", Image, queue_size=queue_size) - bridge = CvBridge() - - # Initialize and configure the sonar - 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(numberOfSamples) - - # Create a new mono-channel image - image = np.zeros((imgSize, imgSize, 1), np.uint8) - - # Center point coordinates - center = (float(imgSize/2),float(imgSize/2)) - while not rospy.is_shutdown(): - 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(numberOfSamples) - p.transmitAngle(angle) - 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 - 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 debug: - cv2.imshow("PolarPlot",image) - cv2.waitKey(27) - else: - cv2.destroyAllWindows() - 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, "mono8")) - except CvBridgeError as e: - print(e) - -# https://discuss.bluerobotics.com/t/please-provide-some-answer-regards-ping360/6393/3?u=stormix - -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, samplePeriod, speedOfSound, _firmwareMinTransmitDuration = 5): - # type: (float, float, int, int) -> 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*getSamplePeriod(samplePeriod)/1000, duration) - # 3 - return max(_firmwareMinTransmitDuration, min(transmitDurationMax(samplePeriod), transmit_duration)) - -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, getSamplePeriod(samplePeriod) * 64e6) - -def getSamplePeriod(samplePeriod, _samplePeriodTickDuration = 25e-9): - # type: (float, float) -> float - """ Sample period in ns """ - return samplePeriod*_samplePeriodTickDuration - - -# Ping 360 Parameters -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 = rospy.get_param('~step', 1) -imgSize = rospy.get_param('~imgSize', 500) -queue_size= rospy.get_param('~queueSize', 1) - -p = Ping360(device, baudrate) -print("Initialized: %s" % p.initialize()) diff --git a/src/ping360_sonar/sensor.py b/src/ping360_sonar/sensor.py index 59b7d5f..5bfd3ee 100644 --- a/src/ping360_sonar/sensor.py +++ b/src/ping360_sonar/sensor.py @@ -1,15 +1,24 @@ -#!/usr/bin/env python +#!/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 device import PingDevice from brping import pingmessage import serial import time class Ping360(PingDevice): + + _gain_setting = 0 + _mode = 0 + _angle = 0 + _transmit_duration = 0 + _sample_period = 0 + _transmit_frequency = 100 + _number_of_samples = 10 + def initialize(self): if not PingDevice.initialize(self): return False @@ -25,23 +34,22 @@ def initialize(self): # @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 + # 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 + # 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") + 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) + "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. + "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 }) @@ -211,9 +219,7 @@ def transmitAngle(self, angle): 1, 0 ) - return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 0.5) + return self.wait_message([definitions.PING360_DEVICE_DATA, definitions.COMMON_NACK], 4.0) def transmit(self): return self.transmitAngle(self._angle) - -