Unverified Commit 080350c4 authored by Anas Mazouni's avatar Anas Mazouni Committed by GitHub
Browse files

Merge pull request #12 from CentraleNantesRobotics/develop

V1.1: Adding a new ROS topic to publish raw sensor data
parents 19f05952 34bbe288
......@@ -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 ##
......
......@@ -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>
<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"/>
......
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
......@@ -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>
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
from polarplot import main
from node import main
from brping.definitions import *
from brping.pingmessage import *
from brping.device import PingDevice
......
#!/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",