Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,6 @@ brping/ping360.py
brping/s500.py
brping/surveyor240.py
brping/omniscan450.py
brping/omniscan3d.py
doc/xml
examples/logs
23 changes: 15 additions & 8 deletions brping/pingmessage.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,11 @@
# PingMessage.py
# Python implementation of the Blue Robotics 'Ping' binary message protocol

import logging
import struct
from brping import definitions

logger = logging.getLogger(__name__)
payload_dict = definitions.payload_dict_all
asciiMsgs = [definitions.COMMON_NACK, definitions.COMMON_ASCII_TEXT]
variable_msgs = [
Expand Down Expand Up @@ -133,9 +136,8 @@ def __init__(self, msg_id=0, msg_data=None, payload_dict=None):
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
except KeyError:
raise

## Pack object attributes into self.msg_data (bytearray)
# @return self.msg_data
Expand Down Expand Up @@ -182,7 +184,7 @@ def unpack_msg_data(self, msg_data):
try:
self.name = self.payload_dict[self.message_id]["name"]
except KeyError:
print("Unknown message: ", self.message_id)
logger.debug("Unknown message: %s", self.message_id)
return False

## The field names of this message
Expand All @@ -196,10 +198,15 @@ def unpack_msg_data(self, msg_data):
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)
logger.debug(
"Error unpacking payload: %s; msg_data=%s; header=%s; format=%s; buf=%s",
e,
msg_data,
header,
PingMessage.endianess + self.payload_format,
self.msg_data[PingMessage.headerLength:PingMessage.headerLength + self.payload_length],
exc_info=True,
)
else: # only use payload if didn't raise exception
for i, attr in enumerate(self.payload_field_names):
try:
Expand Down
22 changes: 15 additions & 7 deletions generate/templates/device.py.in
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,13 @@
from brping import definitions
from brping import pingmessage
from collections import deque
import logging
import serial
import socket
import time

logger = logging.getLogger(__name__)

class PingDevice(object):
{% for field in all_fields|sort %}
_{{field}} = None
Expand Down Expand Up @@ -47,11 +50,10 @@ class PingDevice(object):
#
def connect_serial(self, device_name: str, baudrate: int =115200):
if device_name is None:
print("Device name is required")
return
raise ValueError("Device name is required")

try:
print("Opening %s at %d bps" % (device_name, baudrate))
logger.info("Opening %s at %d bps", device_name, baudrate)

## Serial object for device communication
# write_timeout fixes it getting stuck forever atempting to write to
Expand All @@ -62,7 +64,7 @@ class PingDevice(object):
try:
self.iodev.set_low_latency_mode(True)
except Exception as exception:
print("Failed to set low latency mode: {0}".format(exception))
logger.debug("Failed to set low latency mode: %s", exception, exc_info=True)
self.iodev.send_break()
time.sleep(0.001)
self.iodev.write("U".encode("ascii"))
Expand All @@ -82,7 +84,7 @@ class PingDevice(object):

self.server_address = (host, port)
try:
print("Opening %s:%d" % self.server_address)
logger.info("Opening %s:%d", *self.server_address)
## Serial object for device communication
self.iodev = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.iodev.connect(self.server_address)
Expand Down Expand Up @@ -198,14 +200,20 @@ class PingDevice(object):
self._dst_device_id = msg.dst_device_id

if not hasattr(msg, "payload_field_names"):
print(f"Unrecognized message: {msg.message_id}")
logger.debug("Unrecognized message: %s", msg.message_id)
return False

try:
for attr in msg.payload_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))
logger.debug(
"Attribute error while handling msg %d (%s): %s",
msg.message_id,
msg.name,
msg.msg_data,
exc_info=True,
)
return False

return True
Expand Down
13 changes: 7 additions & 6 deletions generate/templates/omniscan3d.py.in
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
from brping import definitions
from brping import PingDevice
from brping import pingmessage
import logging
import math
import time
import struct
Expand All @@ -23,6 +24,7 @@ import sys
import platform

MAX_LOG_SIZE_MB = 500
logger = logging.getLogger(__name__)

class Omniscan3D(PingDevice):
def __init__(self, logging = False, log_directory = None):
Expand Down Expand Up @@ -253,7 +255,7 @@ class Omniscan3D(PingDevice):
self.logging = True
self.bytes_written = 0

print(f"Logging to {self.current_log}")
logger.info("Logging to %s", self.current_log)

self.write_data(self.build_metadata_packet())

Expand All @@ -271,11 +273,11 @@ class Omniscan3D(PingDevice):
self.bytes_written += len(msg.msg_data)

except (OSError, IOError) as e:
print(f"[LOGGING ERROR] Failed to write to log file {self.current_log}: {e}")
logger.exception("Failed to write to log file %s", self.current_log)
self.stop_logging()

except Exception as e:
print(f"[LOGGING ERROR] Unexpected error: {e}")
logger.exception("Unexpected error while writing to log file %s", self.current_log)
self.stop_logging()

# Override wait_message to also handle point set requests from Omniscan3D and for creating atof_t data
Expand Down Expand Up @@ -308,14 +310,13 @@ class Omniscan3D(PingDevice):

self.server_address = (host, port)
try:
print("Opening %s:%d" % self.server_address)
logger.info("Opening %s:%d", *self.server_address)
self.iodev = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.iodev.settimeout(timeout)
self.iodev.connect(self.server_address)
self.iodev.setblocking(0)

except socket.timeout:
print("Unable to connect to device")
raise Exception("Connection timed out after {0} seconds".format(timeout))
except Exception as exception:
raise Exception("Failed to open the given TCP port: {0}".format(exception))
Expand Down Expand Up @@ -416,4 +417,4 @@ if __name__ == "__main__":
try:
p.iodev.close()
except Exception as e:
print(f"Failed to close socket: {e}")
print(f"Failed to close socket: {e}")
13 changes: 7 additions & 6 deletions generate/templates/omniscan450.py.in
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
from brping import definitions
from brping import PingDevice
from brping import pingmessage
import logging
import time
import math
import struct
Expand All @@ -25,6 +26,7 @@ import sys
import platform

MAX_LOG_SIZE_MB = 500
logger = logging.getLogger(__name__)

class Omniscan450(PingDevice):
def __init__(self, logging = False, log_directory = None):
Expand Down Expand Up @@ -262,7 +264,7 @@ class Omniscan450(PingDevice):
self.logging = True
self.bytes_written = 0

print(f"Logging to {self.current_log}")
logger.info("Logging to %s", self.current_log)

self.write_data(self.build_metadata_packet())

Expand All @@ -280,11 +282,11 @@ class Omniscan450(PingDevice):
self.bytes_written += len(msg.msg_data)

except (OSError, IOError) as e:
print(f"[LOGGING ERROR] Failed to write to log file {self.current_log}: {e}")
logger.exception("Failed to write to log file %s", self.current_log)
self.stop_logging()

except Exception as e:
print(f"[LOGGING ERROR] Unexpected error: {e}")
logger.exception("Unexpected error while writing to log file %s", self.current_log)
self.stop_logging()

# Override wait_message to format power results before returning
Expand Down Expand Up @@ -317,14 +319,13 @@ class Omniscan450(PingDevice):

self.server_address = (host, port)
try:
print("Opening %s:%d" % self.server_address)
logger.info("Opening %s:%d", *self.server_address)
self.iodev = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.iodev.settimeout(timeout)
self.iodev.connect(self.server_address)
self.iodev.setblocking(0)

except socket.timeout:
print("Unable to connect to device")
raise Exception("Connection timed out after {0} seconds".format(timeout))
except Exception as exception:
raise Exception("Failed to open the given TCP port: {0}".format(exception))
Expand Down Expand Up @@ -388,4 +389,4 @@ if __name__ == "__main__":
try:
p.iodev.close()
except Exception as e:
print(f"Failed to close socket: {e}")
print(f"Failed to close socket: {e}")
13 changes: 7 additions & 6 deletions generate/templates/s500.py.in
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
from brping import definitions
from brping import PingDevice
from brping import pingmessage
import logging
import time
import struct
import socket
Expand All @@ -23,6 +24,7 @@ import sys
import platform

MAX_LOG_SIZE_MB = 500
logger = logging.getLogger(__name__)

class S500(PingDevice):
def __init__(self, logging = False, log_directory = None):
Expand Down Expand Up @@ -121,14 +123,13 @@ class S500(PingDevice):

self.server_address = (host, port)
try:
print("Opening %s:%d" % self.server_address)
logger.info("Opening %s:%d", *self.server_address)
self.iodev = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.iodev.settimeout(timeout)
self.iodev.connect(self.server_address)
self.iodev.setblocking(0)

except socket.timeout:
print("Unable to connect to device")
raise Exception("Connection timed out after {0} seconds".format(timeout))
except Exception as exception:
raise Exception("Failed to open the given TCP port: {0}".format(exception))
Expand Down Expand Up @@ -296,7 +297,7 @@ class S500(PingDevice):
self.logging = True
self.bytes_written = 0

print(f"Logging to {self.current_log}")
logger.info("Logging to %s", self.current_log)

self.write_data(self.build_metadata_packet())

Expand All @@ -314,11 +315,11 @@ class S500(PingDevice):
self.bytes_written += len(msg.msg_data)

except (OSError, IOError) as e:
print(f"[LOGGING ERROR] Failed to write to log file {self.current_log}: {e}")
logger.exception("Failed to write to log file %s", self.current_log)
self.stop_logging()

except Exception as e:
print(f"[LOGGING ERROR] Unexpected error: {e}")
logger.exception("Unexpected error while writing to log file %s", self.current_log)
self.stop_logging()

# Override wait_message to format power results before returning
Expand Down Expand Up @@ -367,4 +368,4 @@ if __name__ == "__main__":
try:
p.iodev.close()
except Exception as e:
print(f"Failed to close socket: {e}")
print(f"Failed to close socket: {e}")
13 changes: 7 additions & 6 deletions generate/templates/surveyor240.py.in
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
from brping import definitions
from brping import PingDevice
from brping import pingmessage
import logging
import math
import time
import struct
Expand All @@ -25,6 +26,7 @@ import sys
import platform

MAX_LOG_SIZE_MB = 500
logger = logging.getLogger(__name__)

class Surveyor240(PingDevice):
def __init__(self, logging = False, log_directory = None):
Expand Down Expand Up @@ -254,7 +256,7 @@ class Surveyor240(PingDevice):
self.logging = True
self.bytes_written = 0

print(f"Logging to {self.current_log}")
logger.info("Logging to %s", self.current_log)

self.write_data(self.build_metadata_packet())

Expand All @@ -272,11 +274,11 @@ class Surveyor240(PingDevice):
self.bytes_written += len(msg.msg_data)

except (OSError, IOError) as e:
print(f"[LOGGING ERROR] Failed to write to log file {self.current_log}: {e}")
logger.exception("Failed to write to log file %s", self.current_log)
self.stop_logging()

except Exception as e:
print(f"[LOGGING ERROR] Unexpected error: {e}")
logger.exception("Unexpected error while writing to log file %s", self.current_log)
self.stop_logging()

# Override handle_message to respond to a UTC request from Surveyor
Expand Down Expand Up @@ -327,14 +329,13 @@ class Surveyor240(PingDevice):

self.server_address = (host, port)
try:
print("Opening %s:%d" % self.server_address)
logger.info("Opening %s:%d", *self.server_address)
self.iodev = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.iodev.settimeout(timeout)
self.iodev.connect(self.server_address)
self.iodev.setblocking(0)

except socket.timeout:
print("Unable to connect to device")
raise Exception("Connection timed out after {0} seconds".format(timeout))
except Exception as exception:
raise Exception("Failed to open the given TCP port: {0}".format(exception))
Expand Down Expand Up @@ -434,4 +435,4 @@ if __name__ == "__main__":
try:
p.iodev.close()
except Exception as e:
print(f"Failed to close socket: {e}")
print(f"Failed to close socket: {e}")