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