Merge pull request #12 from CentraleNantesRobotics/develop

V1.1: Adding a new ROS topic to publish raw sensor data

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 ##

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":
+
+		<env name="emulated_sonar" value="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
 <!-- prettier-ignore-end -->
 <!-- ALL-CONTRIBUTORS-LIST:END -->
 
-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)
 

launch/example.launch

 <launch>
+    <env name="emulated_sonar" value="true" />
     <node pkg="ping360_sonar" type="ping360_node" name="ping360_node" output="screen">
         <param name="device" value="/dev/ttyUSB0"/>
         <param name="baudrate" value="115200"/>

msg/SonarEcho.msg

+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

package.xml

@@ -16,6 +16,7 @@
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_export_depend>cv_bridge</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
@@ -25,4 +26,5 @@
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 </package>

src/ping360_sonar/Emulator.py

+
+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<id=0xa81c10, open=%s>( 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 _<field_name> 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

src/ping360_sonar/__init__.py

-from polarplot import main
+from node import main
 from brping.definitions import *
 from brping.pingmessage import *
 from brping.device import PingDevice

src/ping360_sonar/definitions.py

-#!/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",