def get(self, message=None, wait=2):
"""Sends command and returns reply.
Args:
message: Message to expect (default: first to come in).
wait: Seconds to wait until received if a specific message is
required (default: 2).
Returns:
Reply.
Raises:
SonarNotInitialized: Attempt reading serial without opening port.
TimeoutError: Process timed out while waiting for specific message.
"""
# Verify sonar is initialized.
if not self.initialized:
raise exceptions.SonarNotInitialized()
expected_name = None
if message:
expected_name = Message.to_string(message)
rospy.logdebug("Waiting for %s message", expected_name)
# Determine end time.
end = datetime.datetime.now() + datetime.timedelta(seconds=wait)
# Wait until received if a specific message ID is requested, otherwise
# wait forever.
while message is None or datetime.datetime.now() < end:
try:
reply = self.conn.get_reply()
# Update state if mtAlive.
if reply.id == Message.ALIVE:
self.__update_state(reply)
# If first was asked, respond immediately.
if message is None:
return reply
# Otherwise, verify reply ID.
if reply.id == message:
rospy.logdebug("Found %s message", expected_name)
return reply
elif reply.id != Message.ALIVE:
rospy.logwarn("Received unexpected %s message", reply.name)
except e:
# Keep trying.
continue
# Timeout.
rospy.logerr("Timed out before receiving message: %s", expected_name)
raise exceptions.TimeoutError()
class TritechMicron(object):
"""Tritech Micron sonar.
In order for attribute changes to immediately be reflected on the device,
use the set() method with the appropriate keyword.
For example, to setup a 20 m sector scan:
with TritechMicron() as sonar:
sonar.set(continuous=False, range=20)
All angles are in radians and headings are relative to the blue LED. So, if
the sonar were not inverted, 0 radians would be facing forward.
Attributes:
ad_high: Amplitude in db to be mapped to max intensity (0-80 db).
ad_low: Amplitude in dB to be mapped to 0 intensity (0-80 db).
adc8on: True if 8-bit resolution ADC, otherwise 4-bit.
centred: Whether the sonar motor is centred.
clock: Sonar time of the day.
conn: Serial connection.
continuous: True if continuous scan, otherwise sector scan.
gain: Initial gain percentage (0.00-1.00).
has_cfg: Whether the sonar has acknowledged with mtBBUserData.
heading: Current sonar heading in radians.
initialized: Whether the sonar has been initialized with parameters.
inverted: Whether the sonar is mounted upside down.
left_limit: Left limit of sector scan in radians.
mo_time: High speed limit of the motor in units of 10 microseconds.
motoring: Whether the sonar motor is moving.
motor_on: Whether device is powered and motor is primed.
nbins: Number of bins per scan line.
no_params: Whether the sonar needs parameters before it can scan.
up_time: Sonar up time.
port: Serial port.
range: Scan range in meters.
recentering: Whether the sonar is recentering its motor.
right_limit: Right limit of sector scans in radians.
scanning: Whether the sonar is scanning.
scanright: Whether the sonar scanning direction is clockwise.
speed: Speed of sound in medium.
step: Mechanical resolution (Resolution enumeration).
"""
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 __enter__(self):
"""Initializes sonar for first use.
Raises:
SonarNotFound: Sonar port could not be opened.
"""
self.open()
return self
def __exit__(self, type, value, traceback):
"""Cleans up."""
self.close()
def open(self):
"""Initializes sonar connection and sets default properties.
Raises:
SonarNotFound: Sonar port could not be opened.
"""
if not self.conn:
try:
self.conn = Socket(self.port)
except OSError as e:
raise exceptions.SonarNotFound(self.port, e)
# Update properties.
rospy.loginfo("Initializing sonar on %s", self.port)
self.initialized = True
# Reboot to make sure the sonar is clean.
self.send(Message.REBOOT)
self.update()
# Set default properties.
self.set(force=True)
# Wait for settings to go through.
while not self.has_cfg or self.no_params or not self.motor_on or not self.centred:
rospy.loginfo(
"Waiting for configuration: (HAS CFG: %s, NO PARAMS: %s)",
self.has_cfg, self.no_params
)
self.update()
rospy.loginfo("Sonar is ready for use")
def close(self):
"""Closes sonar connection."""
# Reboot first to clear sonar of all parameters.
self.send(Message.REBOOT)
self.conn.close()
self.initialized = False
rospy.loginfo("Closed sonar socket")
def get(self, message=None, wait=2):
"""Sends command and returns reply.
Args:
message: Message to expect (default: first to come in).
wait: Seconds to wait until received if a specific message is
required (default: 2).
Returns:
Reply.
Raises:
SonarNotInitialized: Attempt reading serial without opening port.
TimeoutError: Process timed out while waiting for specific message.
"""
# Verify sonar is initialized.
if not self.initialized:
raise exceptions.SonarNotInitialized()
expected_name = None
if message:
expected_name = Message.to_string(message)
rospy.logdebug("Waiting for %s message", expected_name)
# Determine end time.
end = datetime.datetime.now() + datetime.timedelta(seconds=wait)
# Wait until received if a specific message ID is requested, otherwise
# wait forever.
while message is None or datetime.datetime.now() < end:
try:
reply = self.conn.get_reply()
# Update state if mtAlive.
if reply.id == Message.ALIVE:
self.__update_state(reply)
# If first was asked, respond immediately.
if message is None:
return reply
# Otherwise, verify reply ID.
if reply.id == message:
rospy.logdebug("Found %s message", expected_name)
return reply
elif reply.id != Message.ALIVE:
rospy.logwarn(
"Received unexpected %s message",
reply.name
)
except exceptions.PacketCorrupted, serial.SerialException:
# Keep trying.
continue
# Timeout.
rospy.logerr("Timed out before receiving message: %s", expected_name)
raise exceptions.TimeoutError()
def _handle_windows_timeout():
raise exceptions.TimeoutError(error_message)
class TritechProfiler(object):
""" *TritechProfiler* class for Tritech Profiler sonar.
This class provides all the necesary methods to operate the sonar profiler,
including the sending of commands to configure, scan, ping, or reboot the sensor.
In order for attribute changes to immediately be reflected on the device,
use the set() method with the appropriate keyword.
For example, to setup a 20 m sector scan:
with TritechProfiler() as sonar:
sonar.set(prf_alt=False, range=20)
All angles are in radians and headings are relative to the red LED. So, if
the sonar were not inverted, 0 radians would be facing forward.
**Attributes**:
.. data:: agc
This bit is used to enable/disable the Adaptive Gain Control (AGC) in the sonar receiver. *AGCMax*
and *SetPoint* determine the levels for this control. *AGC* applies automatic refining of the
receiver Gain. If false applies 'manual' Gain (*AGC_Max*).
*AGC* will always act on the maximum received echo, and as a result the ``prf_first`` control will
de-activate when ``agc`` is enabled.
**True** if automatic refining of receiver gain, **False** manual gain.
**Default = True**
.. data:: prf_alt
This bit governs whether Profiler scanning should be alternative as per a car windscreen wiper (i.e.
direction = Scan Alternate), or whether scanning should in one direction only defined by the
``scanright`` bit (Scan Right or Scan Left).
**True** if alternating scan, **False** if scan in one direction.
**Default = False**
.. data:: scanright
This bit determines the scanning direction when ``prf_alt`` (Bit 1) = 0. It is ignored when ``prf_alt`` = 1.
scanright = 0 = Profiler scans anticlockwise when viewed from top (Scan Left).
scanright =1 = Clockwise scan rotation (Scan Right).
**Default = 1**
.. data:: inverted
This bit allows the rotation directions to be reversed if the Profiler head is mounted inverted, i.e.
when the transducer boot is pointing backwards against direction of travel rather than forwards.
**Default = 0 = Profiler mounted 'upright', transducer boot pointing forward.**
.. data:: left_limit
Left limit of sector scan in radians. ``LeftLim`` refers to the Anti-Clockwise scan limit.
``scanright`` = True = 1st ping is at ``left_limit``
``scanright`` = False = 1st ping is at ``right_limit``
The units are in 1/16th Gradian units, in the range [0, 6399]
The SeaKing direction convention is as follows, with transducer boot forwards:
Left 90° = 1600
Ahead = 3200
Right 90° = 4800
Astern = 0 (or 6399)
The ``cont`` bit in ``HdCtrl`` will override these limits, and allow continuous rotation.
The ``stareLLim`` bit in ``HdCtrl`` will cause the head to be driven to the ``LeftLim`` position and
"stare" in that direction. If ``LeftLim`` = ``RightLim``, then the head will act as if the ``stareLLim``
bit is set.
To Scan a 90° sector ahead, set:
LeftLim = 2400 (= Left 45°)
RightLim = 4000 (= Right 45°)
.. data:: right_limit
Left limit of sector scan in radians. ``right_limit`` refers to the Clockwise scan limit.
``scanright`` = True = 1st ping is at ``left_limit``
``scanright`` = False = 1st ping is at ``right_limit``
The units are in 1/16th Gradian units, in the range [0, 6399]
The SeaKing direction convention is as follows, with transducer boot forwards:
Left 90° = 1600
Ahead = 3200
Right 90° = 4800
Astern = 0 (or 6399)
The ``cont`` bit in ``HdCtrl`` will override these limits, and allow continuous rotation.
The ``stareLLim`` bit in ``HdCtrl`` will cause the head to be driven to the ``LeftLim`` position and
"stare" in that direction. If ``LeftLim`` = ``RightLim``, then the head will act as if the ``stareLLim``
bit is set.
To Scan a 90° sector ahead, set:
LeftLim = 2400 (= Left 45°)
RightLim = 4000 (= Right 45°)
.. data:: adc_threshold
This sets the analogue detect threshold level on the receiver and controls the sensitivity of the Profiler.
A higher detect threshold will make the device less sensitive.
**Default = 50**
.. data:: filt_gain
This is the level of Adaptive Gain Control (%) that is applied to the receiver.
**Default = 20**
.. data:: mo_time
High speed limit of the motor in units of 10 microseconds.
**Default = 25 to 40 (for Profiling mode)**
.. data:: step
This byte sets the scanning motor step angle between ‘pings’, and is in units of 1/16 Gradians. The
SeaKing default settings are:
Low Resolution = 32 (= 2 Gradians = 1.8 °)
Medium Resolution = 24 (= 1.5 Gradian = 1.35°)
High Resolution = 16 (= 1 Gradian = 0.9°)
Ultimate Resolution (= 0.5 Gradian = 0.45°)
.. data:: lockout
This value is the time in microseconds at which the Receiver will start sampling after it has transmitted.
The factory default setting for this is:
- Lockout = 271 (for 1100/1210 kHz Profiler channel)
- Lockout = 542 (for 580/600 kHz Profiler channel)
**Default = 100**
.. data:: centred
Whether the sonar motor is centred.
.. data:: clock
Sonar time of the day.
.. data:: conn
Serial connection.
.. data:: gain
Initial gain percentage (0.00-1.00).
.. data:: has_cfg
Whether the sonar has acknowledged with mtBBUserData.
.. data:: heading
Current sonar heading in radians.
.. data:: initialized
Whether the sonar has been initialized with parameters.
.. data:: motoring
Whether the sonar motor is moving.
.. data:: motor_on
Whether device is powered and motor is primed.
.. data:: nbins
Number of bins per scan line.
.. data:: no_params
Whether the sonar needs parameters before it can scan.
.. data:: up_time
Sonar up time.
.. data:: port
Serial port.
.. data:: range
Scan range in meters.
.. data:: recentering
Whether the sonar is recentering its motor.
.. data:: scanning
Whether the sonar is scanning.
.. data:: speed
Speed of sound in medium.
**Default = 1500 m/s**
"""
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 = False
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
self.port_enabled = True
# 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.baudrate = 115200
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 __enter__(self):
"""
Initializes sonar for first use.
Raises:
SonarNotFound: Sonar port could not be opened.
"""
print 'enter'
self.open()
return self
def __exit__(self, type, value, traceback):
"""
Cleans up.
"""
print '__exit__'
self.close()
def reconfigure(self, config, level):
"""Reconfigures sonar dynamically.
Args:
config: New configuration.
level: Level bitmask.
Returns:
Configuration.
"""
rospy.loginfo("Reconfiguring sonar")
rospy.logdebug("Configuration requested: %r, %r", config, level)
# Remove additional keys.
if "groups" in config:
config.pop("groups")
# Set parameters.
self.set(**config)
return config
def open(self):
"""
Initializes sonar connection and sets default properties.
:raises: SonarNotFound: Sonar port could not be opened.
"""
print 'open'
if not self.port_enabled:
return
if not self.conn:
try:
self.conn = Socket(self.port,self.baudrate)
except OSError as e:
raise exceptions.SonarNotFound(self.port, e)
# Update properties.
rospy.loginfo("Initializing sonar on %s", self.port)
self.initialized = True
# Reboot to make sure the sonar is clean.
self.send(Message.REBOOT)
self.update()
self.send(Message.REBOOT)
#rospy.loginfo('Sending Version Data request...')
#self.send(Message.SEND_VERSION)
#self.get(Message.VERSION_DATA, 1)
#rospy.loginfo('Version Data message received')
#self.send(Message.SEND_BB_USER)
#self.get(Message.BB_USER_DATA)
# Set default properties.
self.set(force=True)
# Wait for settings to go through.
while not self.has_cfg or self.no_params:
rospy.loginfo(
"Waiting for configuration: (HAS CFG: %s, NO PARAMS: %s)",
self.has_cfg, self.no_params
)
self.update()
rospy.loginfo("Sonar is ready for use")
def close(self):
"""
Closes sonar connection.
"""
print 'close'
print self.port_enabled
# Reboot first to clear sonar of all parameters.
if self.port_enabled:
self.send(Message.REBOOT)
self.conn.close()
self.initialized = False
else:
print 'switched port, closing profiler'
rospy.loginfo("Closed sonar socket")
def get(self, message=None, wait=4):
"""
Sends command and returns reply.
:param message: Message to expect (default: first to come in).
:param kwargs: wait: Seconds to wait until received if a specific message is required (default: 2).
:return: Reply.
:raises SonarNotInitialized: Attempt reading serial without opening port.
:raises TimeoutError: Process timed out while waiting for specific message.
"""
print 'get'
# Verify sonar is initialized.
if not self.initialized:
raise exceptions.SonarNotInitialized()
expected_name = None
if message:
expected_name = Message.to_string(message)
rospy.logdebug("Waiting for %s message", expected_name)
# Determine end time.
end = datetime.datetime.now() + datetime.timedelta(seconds=wait)
# Wait until received if a specific message ID is requested, otherwise
# wait forever.
while message is None or datetime.datetime.now() < end:
try:
#self.port_enabled = True
print ('valor de port enabled en get reply',self.port_enabled)
if self.port_enabled:
reply = self.conn.get_reply()
# Update state if mtAlive.
if reply.id == Message.ALIVE:
self.__update_state(reply)
# If first was asked, respond immediately.
if message is None:
return reply
# Otherwise, verify reply ID.
if reply.id == message:
rospy.logdebug("Found %s message", expected_name)
return reply
elif reply.id != Message.ALIVE:
rospy.logwarn(
"Received unexpected %s message",
reply.name
)
else:
pass
#print ('port enabled:',self.port_enabled)
#rospy.sleep(0.1)
except exceptions.PacketCorrupted, serial.SerialException:
# Keep trying.
continue
# Timeout.
rospy.logerr("Timed out before receiving message: %s", expected_name)
# if expected_name == 'ALIVE':
# Server(ScanConfig, self.reconfigure)
raise exceptions.TimeoutError()
def _handle_timeout(signum, frame):
raise exceptions.TimeoutError(error_message)