def __init__(self, port="/dev/ttyUSB0", **kwargs):
"""
Constructs Sonar object.
:param port: Serial port (default: /dev/sonar).
:param kwargs: Key-word arguments to pass to set() on initialization
:return:
"""
print 'init'
# Parameter defaults.
self.adc_threshold = 50.0
self.filt_gain = 20.00
self.agc = True
self.prf_alt = False
self.gain = 0.50
self.inverted = False
self.left_limit = to_radians(0)
self.mo_time = 250
self.nbins = 800
self.range = 10.00
self.right_limit = to_radians(6399)
self.scanright = True
self.speed = 1500.0
self.step = Resolution.ULTIMATE
self.lockout = 100
# Override defaults with key-word arguments or ROS parameters.
for key, value in self.__dict__.iteritems():
if key in kwargs:
self.__setattr__(key, value)
else:
param = "{}/{}".format(rospy.get_name(), key)
if rospy.has_param(param):
self.__setattr__(key, rospy.get_param(param))
# Connection properties.
self.port = port
self.conn = None
self.initialized = False
# Head info.
self.centred = False
self.has_cfg = False
self.heading = None
self.motor_on = False
self.motoring = False
self.no_params = True
self.up_time = datetime.timedelta(0)
self.recentering = False
self.scanning = False
# Additional properties.
self.clock = datetime.timedelta(0)
self._time_offset = datetime.timedelta(0)
self.preempted = False
def __init__(self, port="/dev/sonar", **kwargs):
"""Constructs Sonar object.
Args:
port: Serial port (default: /dev/sonar).
kwargs: Key-word arguments to pass to set() on initialization.
"""
# Parameter defaults.
self.ad_high = 80.0
self.ad_low = 0.00
self.adc8on = True
self.continuous = True
self.gain = 0.50
self.inverted = True
self.left_limit = to_radians(2400)
self.mo_time = 250
self.nbins = 400
self.range = 10.00
self.right_limit = to_radians(4000)
self.scanright = True
self.speed = 1500.0
self.step = Resolution.LOW
self.dynamic_reconfigure_started = False
# Override defaults with key-word arguments or ROS parameters.
for key, value in self.__dict__.iteritems():
if key in kwargs:
self.__setattr__(key, value)
else:
param = "{}/{}".format(rospy.get_name(), key)
if rospy.has_param(param):
self.__setattr__(key, rospy.get_param(param))
# Connection properties.
self.port = port
self.conn = None
self.initialized = False
# Head info.
self.centred = False
self.has_cfg = False
self.heading = None
self.motor_on = False
self.motoring = False
self.no_params = True
self.up_time = datetime.timedelta(0)
self.recentering = False
self.scanning = False
# Additional properties.
self.clock = datetime.timedelta(0)
self._time_offset = datetime.timedelta(0)
self.preempted = False
def __update_state(self, alive):
"""Updates Sonar states from mtAlive message.
Args:
alive: mtAlive reply.
"""
payload = alive.payload
payload.bytepos = 1
# Get current time and compute up time.
micros = payload.read(32).uintle * 1000
self.clock = datetime.timedelta(microseconds=micros)
if self._time_offset > self.clock:
self._time_offset = datetime.timedelta(0)
self.up_time = self.clock - self._time_offset
# Get heading.
self.heading = to_radians(payload.read(16).uintle)
# Decode HeadInf byte.
head_inf = payload.read(8)
self.recentering = head_inf[7]
self.centred = head_inf[6]
self.motoring = head_inf[5]
self.motor_on = head_inf[4]
self.scanright = head_inf[3]
self.scanning = head_inf[2]
self.no_params = head_inf[1]
# self.has_cfg = head_inf[0]
# bypass the has_cfg parameter since we don't send mtSendBBUser
self.has_cfg = True
rospy.loginfo("UP TIME: %s", self.up_time)
rospy.logdebug("RECENTERING: %s", self.recentering)
rospy.logdebug("CENTRED: %s", self.centred)
rospy.logdebug("MOTORING: %s", self.motoring)
rospy.logdebug("MOTOR ON: %s", self.motor_on)
rospy.logdebug("CLOCKWISE: %s", self.scanright)
rospy.logdebug("SCANNING: %s", self.scanning)
rospy.logdebug("NO PARAMS: %s", self.no_params)
rospy.logdebug("HAS CFG: %s", self.has_cfg)
class Resolution(object):
"""Sonar mechanical resolution enumeration.
The mechanical resolution is the angle the step motor rotates per scan line
in radians. A higher resolution slows down the scan.
Set as such:
with TritechMicron() as sonar:
sonar.set(step=Resolution.LOW)
Other resolutions can also be used, but these are the ones documented by
Tritech.
"""
LOWEST = to_radians(255) # Not recommended.
LOWER = to_radians(128) # Not recommended.
LOWERISH = to_radians(64) # Not recommended.
LOW = to_radians(32)
MEDIUM = to_radians(16)
HIGH = to_radians(8)
ULTIMATE = to_radians(4)
def __parse_head_data(self, data):
"""Parses mtHeadData payload and returns parsed bins.
Args:
data: mtHeadData bitstring.
Returns:
Bins.
Raise:
ValueError: If data could not be parsed.
"""
# Any number of exceptions could occur here if the packet is corrupted,
# so a catch-all approach is used for safety.
try:
# Get the total number of bytes.
count = data.read(16).uintle
rospy.logdebug("Byte count is %d", count)
# The device type should be 0x11 for a DST Sonar.
device_type = data.read(8)
if device_type.uint != 0x11:
# Packet is likely corrupted, try again.
raise ValueError(
"Unexpected device type: {}"
.format(device_type.hex)
)
# Get the head status byte:
# Bit 0: 'HdPwrLoss'. Head is in Reset Condition.
# Bit 1: 'MotErr'. Motor has lost sync, re-send Parameters.
# Bit 2: 'PrfSyncErr'. Always 0.
# Bit 3: 'PrfPingErr'. Always 0.
# Bit 4: Whether adc8on is enabled.
# Bit 5: RESERVED (ignore).
# Bit 6: RESERVED (ignore).
# Bit 7: Message appended after last packet data reply.
_head_status = data.read(8)
rospy.logdebug("Head status byte is %s", _head_status)
if _head_status[-1]:
rospy.logerr("Head power loss detected")
if _head_status[-2]:
rospy.logerr("Motor lost sync")
self.set(force=True)
# Get the sweep code. Its value should correspond to:
# 0: Scanning normal.
# 1: Scan at left limit.
# 2: Scan at right limit.
# 3: RESERVED (ignore).
# 4: RESERVED (ignore)
# 5: Scan at center position.
sweep = data.read(8).uint
rospy.logdebug("Sweep code is %d", sweep)
if sweep == 1:
rospy.loginfo("Reached left limit")
elif sweep == 2:
rospy.loginfo("Reached right limit")
# Get the HdCtrl bytes to control operation:
# Bit 0: adc8on 0: 4-bit 1: 8-bit
# Bit 1: cont 0: sector-scan 1: continuous
# Bit 2: scanright 0: left 1: right
# Bit 3: invert 0: upright 1: inverted
# Bit 4: motoff 0: on 1: off
# Bit 5: txoff 0: on 1: off (for testing)
# Bit 6: spare 0: default 1: N/A
# Bit 7: chan2 0: default 1: N/A
# Bit 8: raw 0: N/A 1: default
# Bit 9: hasmot 0: lol 1: has a motor (always)
# Bit 10: applyoffset 0: default 1: heading offset
# Bit 11: pingpong 0: default 1: side-scanning sonar
# Bit 12: stareLLim 0: default 1: N/A
# Bit 13: ReplyASL 0: N/A 1: default
# Bit 14: ReplyThr 0: default 1: N/A
# Bit 15: IgnoreSensor 0: default 1: emergencies
# Should be the same as what was sent.
hd_ctrl = data.read(16)
hd_ctrl.byteswap() # Little endian please.
self.inverted, self.scanright, self.continuous, self.adc8on = (
hd_ctrl.unpack("pad:12, bool, bool, bool, bool")
)
rospy.logdebug("Head control bytes are %s", hd_ctrl.bin)
rospy.logdebug("ADC8 mode %s", self.adc8on)
rospy.logdebug("Continuous mode %s", self.continuous)
rospy.logdebug("Scanning right %s", self.scanright)
# Range scale.
# The lower 14 bits are the range scale * 10 units and the higher 2
# bits are coded units:
# 0: meters
# 1: feet
# 2: fathoms
# 3: yards
# Only the metric system is implemented for now, because it is
# better.
self.range = data.read(16).uintle / 10.0
rospy.logdebug("Range scale is %f", self.range)
# TX/RX transmitter constants: N/A to DST.
data.read(32)
# The gain ranges from 0 to 210.
self.gain = data.read(8).uintle / 210.0
rospy.logdebug("Gain is %f", self.gain)
# Slope setting is N/A to DST.
data.read(16)
# If the ADC is set to 8-bit, MAX = 255 else MAX = 15.
# ADLow = MAX * low / 80 where low is the desired minimum
# amplitude.
# ADSpan = MAX * span / 80 where span is the desired amplitude
# span.
# The full range is between ADLow and ADLow + ADSpan.
MAX_SIZE = 255 if self.adc8on else 15
ad_span = data.read(8).uintle
ad_low = data.read(8).uintle
self.ad_low = ad_low * 80.0 / MAX_SIZE
span_intensity = ad_span * 80.0 / MAX_SIZE
self.ad_high = self.ad_low + span_intensity
rospy.logdebug("AD range is %f to %f", self.ad_low, self.ad_high)
# Heading offset is ignored.
heading_offset = to_radians(data.read(16).uint)
rospy.logdebug("Heading offset is %f", heading_offset)
# ADInterval defines the sampling interval of each bin and is in
# units of 640 nanoseconds.
ad_interval = data.read(16).uintle
rospy.logdebug("AD interval is %d", ad_interval)
# Left/right angles limits are in 1/16th of a gradian.
self.left_limit = to_radians(data.read(16).uintle)
self.right_limit = to_radians(data.read(16).uintle)
rospy.logdebug(
"Limits are %f to %f",
self.left_limit, self.right_limit
)
# Step angle size.
self.step = to_radians(data.read(8).uint)
rospy.logdebug("Step size is %f", self.step)
# Heading is in units of 1/16th of a gradian.
self.heading = to_radians(data.read(16).uintle)
rospy.loginfo("Heading is now %f", self.heading)
# Dbytes is the number of bytes with data to follow.
dbytes = data.read(16).uintle
if self.adc8on:
self.nbins = dbytes
bin_size = 8
else:
self.nbins = dbytes * 2
bin_size = 4
rospy.logdebug("DBytes is %d", dbytes)
# Get bins.
bins = [data.read(bin_size).uint for i in range(self.nbins)]
except Exception as e:
# Damn.
raise ValueError(e)
return bins
def __parse_head_data(self, data):
"""Parses mtHeadData payload and returns parsed bins.
Args:
data: mtHeadData bitstring.
Returns:
Bins.
Raise:
ValueError: If data could not be parsed.
"""
# Any number of exceptions could occur here if the packet is corrupted,
# so a catch-all approach is used for safety.
try:
# Get the total number of bytes.
count = data.read(16).uintle
rospy.logdebug("Byte count is %d", count)
# The device type should be 0x11 for a DST Sonar.
device_type = data.read(8)
if device_type.uint != 0x11:
# Packet is likely corrupted, try again.
raise ValueError("Unexpected device type: {}".format(
device_type.hex))
# Get the head status byte:
# Bit 0: 'HdPwrLoss'. Head is in Reset Condition.
# Bit 1: 'MotErr'. Motor has lost sync, re-send Parameters.
# Bit 2: 'PrfSyncErr'. Always 0.
# Bit 3: 'PrfPingErr'. Always 0.
# Bit 4: Whether adc8on is enabled.
# Bit 5: RESERVED (ignore).
# Bit 6: RESERVED (ignore).
# Bit 7: Message appended after last packet data reply.
_head_status = data.read(8)
rospy.logdebug("Head status byte is %s", _head_status)
if _head_status[-1]:
rospy.logerr("Head power loss detected")
if _head_status[-2]:
rospy.logerr("Motor lost sync")
self.set(force=True)
# Get the sweep code. Its value should correspond to:
# 0: Scanning normal.
# 1: Scan at left limit.
# 2: Scan at right limit.
# 3: RESERVED (ignore).
# 4: RESERVED (ignore)
# 5: Scan at center position.
sweep = data.read(8).uint
rospy.logdebug("Sweep code is %d", sweep)
if sweep == 1:
rospy.loginfo("Reached left limit")
elif sweep == 2:
rospy.loginfo("Reached right limit")
# Get the HdCtrl bytes to control operation:
# Bit 0: adc8on 0: 4-bit 1: 8-bit
# Bit 1: cont 0: sector-scan 1: continuous
# Bit 2: scanright 0: left 1: right
# Bit 3: invert 0: upright 1: inverted
# Bit 4: motoff 0: on 1: off
# Bit 5: txoff 0: on 1: off (for testing)
# Bit 6: spare 0: default 1: N/A
# Bit 7: chan2 0: default 1: N/A
# Bit 8: raw 0: N/A 1: default
# Bit 9: hasmot 0: lol 1: has a motor (always)
# Bit 10: applyoffset 0: default 1: heading offset
# Bit 11: pingpong 0: default 1: side-scanning sonar
# Bit 12: stareLLim 0: default 1: N/A
# Bit 13: ReplyASL 0: N/A 1: default
# Bit 14: ReplyThr 0: default 1: N/A
# Bit 15: IgnoreSensor 0: default 1: emergencies
# Should be the same as what was sent.
hd_ctrl = data.read(16)
hd_ctrl.byteswap() # Little endian please.
self.inverted, self.scanright, self.continuous, self.adc8on = (
hd_ctrl.unpack("pad:12, bool, bool, bool, bool"))
rospy.logdebug("Head control bytes are %s", hd_ctrl.bin)
rospy.logdebug("ADC8 mode %s", self.adc8on)
rospy.logdebug("Continuous mode %s", self.continuous)
rospy.logdebug("Scanning right %s", self.scanright)
# Range scale.
# The lower 14 bits are the range scale * 10 units and the higher 2
# bits are coded units:
# 0: meters
# 1: feet
# 2: fathoms
# 3: yards
# Only the metric system is implemented for now, because it is
# better.
self.range = data.read(16).uintle / 10.0
rospy.logdebug("Range scale is %f", self.range)
# TX/RX transmitter constants: N/A to DST.
data.read(32)
# The gain ranges from 0 to 210.
self.gain = data.read(8).uintle / 210.0
rospy.logdebug("Gain is %f", self.gain)
# Slope setting is N/A to DST.
data.read(16)
# If the ADC is set to 8-bit, MAX = 255 else MAX = 15.
# ADLow = MAX * low / 80 where low is the desired minimum
# amplitude.
# ADSpan = MAX * span / 80 where span is the desired amplitude
# span.
# The full range is between ADLow and ADLow + ADSpan.
MAX_SIZE = 255 if self.adc8on else 15
ad_span = data.read(8).uintle
ad_low = data.read(8).uintle
self.ad_low = ad_low * 80.0 / MAX_SIZE
span_intensity = ad_span * 80.0 / MAX_SIZE
self.ad_high = self.ad_low + span_intensity
rospy.logdebug("AD range is %f to %f", self.ad_low, self.ad_high)
# Heading offset is ignored.
heading_offset = to_radians(data.read(16).uint)
rospy.logdebug("Heading offset is %f", heading_offset)
# ADInterval defines the sampling interval of each bin and is in
# units of 640 nanoseconds.
ad_interval = data.read(16).uintle
rospy.logdebug("AD interval is %d", ad_interval)
# Left/right angles limits are in 1/16th of a gradian.
self.left_limit = to_radians(data.read(16).uintle)
self.right_limit = to_radians(data.read(16).uintle)
rospy.logdebug("Limits are %f to %f", self.left_limit,
self.right_limit)
# Step angle size.
self.step = to_radians(data.read(8).uint)
rospy.logdebug("Step size is %f", self.step)
# Heading is in units of 1/16th of a gradian.
self.heading = to_radians(data.read(16).uintle)
rospy.loginfo("Heading is now %f", self.heading)
# Dbytes is the number of bytes with data to follow.
dbytes = data.read(16).uintle
if self.adc8on:
self.nbins = dbytes
bin_size = 8
else:
self.nbins = dbytes * 2
bin_size = 4
rospy.logdebug("DBytes is %d", dbytes)
# Get bins.
bins = [data.read(bin_size).uint for i in range(self.nbins)]
except Exception as e:
# Damn.
raise ValueError(e)
return bins
def __parse_head_data(self, data):
"""
Parses mtHeadData payload and returns parsed bins.
:param mtHeadData bitstring.
:return: Bins.
:raises ValueError: If data could not be parsed.
"""
print '_parse_head:data'
# Any number of exceptions could occur here if the packet is corrupted,
# so a catch-all approach is used for safety.
try:
# Get the total number of bytes.
count = data.read(16).uintle
rospy.logdebug("Byte count is %d", count)
# The device type should be 0x05 for a Profiling Sonar.
device_type = data.read(8)
if device_type.uint != 0x05:
# Packet is likely corrupted, try again.
raise ValueError(
"Unexpected device type: {}"
.format(device_type.hex)
)
# Get the head status byte:
# Bit 0: 'HdPwrLoss'. Head is in Reset Condition.
# Bit 1: 'MotErr'. Motor has lost sync, re-send Parameters.
# Bit 2: 'PrfSyncErr'. Always 0.
# Bit 3: 'PrfPingErr'. Always 0.
# Bit 4: Whether prf_AGC is enabled.
# Bit 5: RESERVED (ignore).
# Bit 6: RESERVED (ignore).
# Bit 7: Message appended after last packet data reply.
_head_status = data.read(8)
rospy.logdebug("Head status byte is %s", _head_status)
if _head_status[-1]:
rospy.logerr("Head power loss detected")
if _head_status[-2]:
rospy.logerr("Motor lost sync")
self.set(force=True)
# Get the sweep code. Its value should correspond to:
# 0: Scanning normal.
# 1: Scan at left limit.
# 2: Scan at right limit.
# 3: RESERVED (ignore).
# 4: RESERVED (ignore)
# 5: Scan at center position.
sweep = data.read(8).uint
rospy.logdebug("Sweep code is %d", sweep)
if sweep == 1:
rospy.loginfo("Reached left limit")
elif sweep == 2:
rospy.loginfo("Reached right limit")
# Get the HdCtrl bytes to control operation:
# Bit 0: prf_AGC 0: AGC off(def) 1: AGC on
# Bit 1: prf_alt 0: continuous 1: alternate scan
# Bit 2: scanright 0: left 1: right
# Bit 3: invert 0: upright 1: inverted
# Bit 4: motoff 0: on 1: off
# Bit 5: txoff 0: on 1: off (for testing)
# Bit 6: prf_t10 0: default 1: N/A
# Bit 7: chan2 0: default 1: N/A
# Bit 8: prf_first 0: N/A 1: default
# Bit 9: hasmot 0: lol 1: has a motor (always)
# Bit 10: prf_PingSync 0: sync off 1: sync on, default
# Bit 11: prf_ScanSync 0: default 1: use when Dual Head Profiler
# Bit 12: stareLLim 0: default 1: N/A
# Bit 13: prf_Master 0: N/A 1: default for single profiler
# Bit 14: prf_Mirror 0: default 1: N/A
# Bit 15: IgnoreSensor 0: default 1: emergencies
# Should be the same as what was sent.
hd_ctrl = data.read(16)
hd_ctrl.byteswap() # Little endian please.
self.inverted, self.scanright, self.prf_alt, self.prf_AGC = (
hd_ctrl.unpack("pad:12, bool, bool, bool, bool")
)
rospy.logdebug("Head control bytes are %s", hd_ctrl.bin)
rospy.logdebug("PRF Adaptive Gain Control mode %s", self.prf_AGC)
rospy.logdebug("PRF Alternate Scan mode %s", self.prf_alt)
rospy.logdebug("Scanning right %s", self.scanright)
# Range scale.
# The lower 14 bits are the range scale * 10 units and the higher 2
# bits are coded units:
# 0: meters
# 1: feet
# 2: fathoms
# 3: yards
# Only the metric system is implemented for now, because it is
# better.
self.range = data.read(16).uintle / 10.0
rospy.logdebug("Range scale is %f", self.range)
# TX/RX transmitter constants: N/A to DST.
data.read(32)
# The gain ranges from 0 to 210.
self.gain = data.read(8).uintle
rospy.logdebug("Gain is %f", self.gain)
# Slope setting is N/A to DST.
data.read(16)
# ADC analog threshold (1/255 units)
adc_threshold = data.read(8).uintle
filt_gain = data.read(8).uintle
rospy.logdebug("filt Gain is %f", filt_gain)
# Left/right angles limits are in 1/16th of a gradian.
self.left_limit = to_radians(data.read(16).uintle)
self.right_limit = to_radians(data.read(16).uintle)
rospy.logdebug(
"Limits are %f to %f",
self.left_limit, self.right_limit
)
# Step angle size.
self.step = to_radians(data.read(8).uint)
rospy.logdebug("Step size is %f", self.step)
ScanTime = data.read(16).uintle
rospy.logdebug("Scan Time is %d", ScanTime)
# Dbytes is the number of bytes with data to follow.
dbytes = data.read(16).uintle
self.nbins = dbytes
bin_size = 16 # antes 8
rospy.logdebug("DBytes is %d", dbytes)
# Get bins.
bins = [data.read(bin_size).uintle for i in range(self.nbins)]
except Exception as e:
# Damn.
raise ValueError(e)
return bins
