def get(self, message=None, wait=2):
"""
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
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)
#print ('port enabled:',self.port_enabled)
#rospy.sleep(0.1)
except exceptions.PacketCorrupted, serial.SerialException:
# Keep trying.
continue
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()
def send(self, command, payload=None):
"""Sends command and returns reply.
Args:
command: Command to send.
Raises:
SonarNotInitialized: Attempt sending command without opening port.
"""
if not self.initialized:
raise exceptions.SonarNotInitialized(command, payload)
self.conn.send(command, payload)
def send(self, command, payload=None):
"""
Sends command and returns reply.
:param command: Command to send.
:param payload: Fields of the command packet
:return: Reply
:raises SonarNotInitialized: Attempt sending command without opening port.
"""
print 'send'
if not self.initialized and self.port_enabled:
raise exceptions.SonarNotInitialized(command, payload)
self.conn.send(command, payload)
def set(self, adc8on=None, continuous=None, scanright=None, step=None,
ad_low=None, ad_high=None, left_limit=None, right_limit=None,
mo_time=None, range=None, nbins=None, gain=None, speed=None,
inverted=None, force=False):
"""Sends Sonar head command with new properties if needed.
Only the parameters specified will be modified.
If initialized, arguments are compared to current properties in order
to see if sending the command is necessary.
Args:
ad_low: Amplitude in dB to be mapped to 0 intensity (0-80 db).
ad_high: Amplitude in db to be mapped to max intensity (0-80 db).
adc8on: True if 8-bit resolution ADC, otherwise 4-bit.
continuous: True if continuous scan, otherwise sector scan.
gain: Initial gain percentage (0.00-1.00).
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.
nbins: Number of bins per scan line.
range: Scan range in meters.
right_limit: Right limit of sector scans in radians.
scanright: Whether the sonar scanning direction is clockwise.
speed: Speed of sound in medium.
step: Mechanical resolution (Resolution enumeration).
force: Whether to force setting the parameters or not.
Raises:
SonarNotInitialized: Sonar is not initialized.
"""
if not self.initialized:
raise exceptions.SonarNotInitialized()
self.__set_parameters(
adc8on=adc8on, continuous=continuous, scanright=scanright,
step=step, ad_low=ad_low, ad_high=ad_high, left_limit=left_limit,
right_limit=right_limit, mo_time=mo_time, range=range, nbins=nbins,
gain=gain, speed=speed, inverted=inverted, force=force
)
def set(self,
agc=None,
prf_alt=None,
scanright=None,
step=None,
filt_gain=None,
adc_threshold=None,
left_limit=None,
right_limit=None,
mo_time=None,
range=None,
gain=None,
speed=None,
lockout=None,
inverted=None,
force=False,
port_enabled=True,
port_baudrate=115200):
"""
Sends Sonar head command with new properties if needed.
Only the parameters specified will be modified.
If initialized, arguments are compared to current properties in order
to see if sending the command is necessary.
:param filt_gain: Percentage of Adaptive Gain Control applied to receiver.
:param adc_threshold: Analogue detect threshold level.
:param agc: True if automatic AGC, otherwise manual AGC.
:param prf_alt: True if alternate scan, otherwise continuous scan.
:param gain: Initial gain percentage (0.00-1.00).
:param inverted: Whether the sonar is mounted upside down.
:param left_limit: Left limit of sector scan in radians.
:param mo_time: High speed limit of the motor in units of 10 microseconds.
:param range: Scan range in meters.
:param right_limit: Right limit of sector scans in radians.
:param scanright: Whether the sonar scanning direction is clockwise.
:param speed: Speed of sound in medium.
:param step: Mechanical resolution (Resolution enumeration).
:param force: Whether to force setting the parameters or not.
:param port_enabled: enables/disables virtual serial port
:param port_baudrate: stablishes port baudrate for communication
:raises SonarNotInitialized: Sonar is not initialized.
"""
print 'set'
if not self.initialized:
raise exceptions.SonarNotInitialized()
self.__set_parameters(agc=agc,
prf_alt=prf_alt,
scanright=scanright,
step=step,
filt_gain=filt_gain,
adc_threshold=adc_threshold,
left_limit=left_limit,
right_limit=right_limit,
mo_time=mo_time,
range=range,
gain=gain,
speed=speed,
lockout=lockout,
inverted=inverted,
force=force,
port_enabled=port_enabled,
port_baudrate=port_baudrate)
