def process_observation(self, obs: Observation) -> Observation:
request_sets = obs.get('requestSets')
for set_name, request_set in request_sets.items():
request = request_set.get('request')
timeout = request_set.get('timeout')
sleep_time = request_set.get('sleepTime')
response = ''
self.logger.verbose(f'Sending request "{set_name}" to sensor '
f'"{obs.get("sensorName")}" on virtual port '
f'"{self.name}"')
for pattern in self.patterns:
reg_exp = re.compile(pattern)
parsed = reg_exp.match(request)
if not parsed:
continue
response = self.patterns[pattern](request)
self.logger.verbose(f'Received response '
f'"{self.sanitize(response)}" from sensor '
f'"{obs.get("sensorName")}" on virtual '
f'port "{self.name}"')
break
request_set['response'] = response
time.sleep((timeout * 0.25) + sleep_time)
obs.set('portName', self._name)
obs.set('timestamp', str(arrow.utcnow()))
return obs
def process_observation(self, obs: Observation) -> Observation:
for response_set in self._response_sets:
return_code = obs.get_value('responseSets', response_set, 'value')
if return_code is None:
continue
if return_code == 0:
if obs.get('corrupted') is True:
obs.set('corrupted', False)
continue
# Get error message of the return code.
error_values = ReturnCodes.codes.get(return_code)
attempts = obs.get('attempts', 0)
# Retry measurement.
if error_values and attempts < self._retries:
obs.set('attempts', attempts + 1)
obs.set('corrupted', False)
obs.set('nextReceiver', 0)
self.logger.info(
f'Retrying observation "{obs.get("name")}" of '
f'target "{obs.get("target")}" due to return '
f'code {return_code} of response '
f'"{response_set}" (attempt {attempts + 1,} '
f'of {self._retries})')
else:
obs.set('corrupted', True)
if error_values:
# Return code related log message.
lvl, retry, msg = error_values
self.logger.log(
lvl * 10, f'Observation "{obs.get("name")}" of '
f'target "{obs.get("target")}": {msg} '
f'(code {return_code} in response '
f'"{response_set}")')
else:
# Generic log message.
self.logger.error(f'Error occurred on observation '
f'"{obs.get("name")}" (unknown code '
f'{return_code} in response '
f'"{response_set}")')
return obs
return obs
def publish_observation(self, obs: Observation) -> None:
"""Prepares the observation for publishing and forwards it to the
messenger.
Args:
obs: Observation object.
"""
receivers = obs.get('receivers')
index = obs.get('nextReceiver')
# No receivers defined.
if not receivers:
logging.debug(f'No receivers defined in observation '
f'"{obs.get("name")}" of target '
f'"{obs.get("target")}"')
return
# No index defined.
if (index is None) or (index < 0):
self.logger.warning(f'Undefined receiver in observation '
f'"{obs.get("name")}" of target '
f'"{obs.get("target")}"')
return
# Receivers list has been processed and observation is finished.
if index >= len(receivers):
self.logger.info(f'Observation "{obs.get("name")}" of target '
f'"{obs.get("target")}" has been finished')
return
# Name of the sending module.
sender = receivers[index - 1]
# Increase the receivers index.
next_receiver = receivers[index]
obs.set('nextReceiver', index + 1)
# Create header and payload.
header = {'from': sender, 'type': 'observation'}
payload = obs.data
# Send the observation to the next module.
self.publish(next_receiver, header, payload)
def process_observation(self,
obs: Observation) -> Union[Observation, None]:
"""Sends a request to the attached sensor and forwards the response.
Args:
obs: Observation object.
Returns:
The extended observation object or None if connection failed.
"""
if not self._sock:
# Open socket connection.
if not self._open():
return
# Add the name of the Bluetooth port to the observation.
obs.set('portName', self.name)
requests_order = obs.get('requestsOrder', [])
request_sets = obs.get('requestSets')
if not requests_order:
self.logger.notice(f'No requests order defined in observation '
f'"{obs.get("name")}" of target '
f'"{obs.get("target")}"')
# Send requests sequentially to the sensor.
for request_name in requests_order:
request_set = request_sets.get(request_name)
if not request_set:
self.logger.error(f'Request set "{request_name}" not found in '
f'observation "{obs.get("name")}" of target '
f'"{obs.get("target")}"')
return
# The response of the sensor.
response = ''
response_delimiter = request_set.get('responseDelimiter')
# Data of the request set.
request = request_set.get('request')
sleep_time = request_set.get('sleepTime') or 1.0
timeout = request_set.get('timeout') or 1.0
# Send the request of the observation to the attached sensor.
self.logger.verbose(f'Sending request "{request_name}" of '
f'observation "{obs.get("name")}" to sensor '
f'"{obs.get("sensorName")}" ...')
# Write to Bluetooth port.
self._send(request)
# Get the response of the sensor.
response = self._receive(response_delimiter, timeout)
self.logger.verbose(f'Received response '
f'"{self.sanitize(response)}" for request '
f'"{request_name}" of observation '
f'"{obs.get("name")}" from sensor '
f'"{obs.get("sensorName")}"')
# Add the raw response of the sensor to the observation set.
request_set['response'] = response
# Add the timestamp to the observation.
obs.set('timestamp', str(arrow.utcnow()))
# Sleep until the next request.
time.sleep(sleep_time)
return obs
def process_observation(self,
obs: Observation) -> Union[Observation, None]:
"""Processes an observation object. Sends request to sensor and stores
response.
Args:
obs: The observation object.
Returns:
The processed observation.
"""
# Turn on passive mode.
if obs.get('passiveMode'):
if self._is_passive:
# Restart passive listener.
self._is_passive = False
self._passive_listener.join()
self._is_passive = True
self._passive_listener = Thread(target=self.listen, args=(obs, ))
self._passive_listener.daemon = True
self._passive_listener.start()
self.logger.debug(
f'Started passive listener of port "{self.name}"')
return
# Turn off passive mode.
if self._is_passive and not obs.get('passiveMode'):
self._is_passive = False
self._passive_listener.join()
self.logger.debug(
f'Stopped passive listener of port "{self.name}"')
# Create new serial connection.
if not self._serial:
self._create()
if self._serial is None:
self.logger.error(f'Could not access port '
f'"{self._serial_port_config.port}"')
return
if not self._serial.is_open:
self.logger.verbose(f'Re-opening port '
f'"{self._serial_port_config.port}" ...')
self._serial.open()
self._serial.reset_output_buffer()
self._serial.reset_input_buffer()
# Add the name of this serial port module to the observation.
obs.set('portName', self._name)
requests_order = obs.get('requestsOrder', [])
request_sets = obs.get('requestSets')
if not requests_order:
self.logger.notice(f'No requests order defined in observation '
f'"{obs.get("name")}" of target '
f'"{obs.get("target")}"')
# Send requests sequentially to the sensor.
for request_name in requests_order:
request_set = request_sets.get(request_name)
if not request_set:
self.logger.error(f'Request set "{request_name}" not found in '
f'observation "{obs.get("name")}" of target '
f'"{obs.get("target")}"')
return
# The response of the sensor.
response = ''
response_delimiter = request_set.get('responseDelimiter')
# Data of the request set.
request = request_set.get('request')
sleep_time = request_set.get('sleepTime') or 0.0
timeout = request_set.get('timeout') or 0.0
# Send the request of the observation to the attached sensor.
self.logger.verbose(f'Sending request "{request_name}" of '
f'observation "{obs.get("name")}" to sensor '
f'"{obs.get("sensorName")}" ...')
for attempt in range(self._max_attempts):
if attempt > 0:
self.logger.info(f'Request attempt {attempt + 1} of '
f'{self._max_attempts}')
time.sleep(1)
# Write to the serial port.
self._write(request)
# Get the response of the sensor.
response = self._read(eol=response_delimiter,
length=0,
timeout=timeout)
self._serial.reset_output_buffer()
self._serial.reset_input_buffer()
if response:
self.logger.verbose(
f'Received response '
f'"{self.sanitize(response)}" for '
f'request "{request_name}" of '
f'observation "{obs.get("name")}" from '
f'sensor "{obs.get("sensorName")}"')
break
# Try next attempt if response is empty.
self.logger.warning(f'No response from sensor '
f'"{obs.get("sensorName")}" for '
f'observation "{obs.get("name")}" of '
f'target "{obs.get("target")}"')
# Add the raw response of the sensor to the observation set.
request_set['response'] = response
# Add the timestamp to the observation.
obs.set('timestamp', str(arrow.utcnow()))
# Sleep until the next request.
time.sleep(sleep_time)
return obs
def _calculate_view_point(self, obs: Obs) -> Union[Obs, None]:
"""Calculates the view point by doing a 2D Helmert transformation.
Args:
obs: `Observation` object. Needed for port and sensor information.
Returns:
New `Observation` object with view point coordinates.
"""
sum_local_x = sum_local_y = sum_local_z = 0 # [x], [y], [z].
sum_global_x = sum_global_y = sum_global_z = 0 # [X], [Y], [Z].
num_fixed_points = len(self._fixed_points) # n.
# Calculate the centroid coordinates of the view point.
for name, fixed_point in self._fixed_points.items():
hz = fixed_point.get('hz') # Horizontal direction.
v = fixed_point.get('v') # Vertical angle.
dist = fixed_point.get('dist') # Distance (slope or reduced).
if None in [hz, v, dist]:
self.logger.warning(f'Hz, V, or distance missing in fixed '
f'point "{name}"')
return
# Calculate Cartesian coordinates out of polar coordinates.
local_x, local_y, local_z = self.get_cartesian_coordinates(
hz, v, dist)
# Store local coordinates in the fixed point dictionary.
fixed_point['localX'] = local_x
fixed_point['localY'] = local_y
fixed_point['localZ'] = local_z
# Coordinates in the global system (X, Y, Z).
global_x = fixed_point.get('x')
global_y = fixed_point.get('y')
global_z = fixed_point.get('z')
if None in [global_x, global_y, global_z]:
self.logger.error(f'Undefined fixed point "{name}"')
# Sums of the coordinates.
sum_local_x += local_x
sum_local_y += local_y
sum_local_z += local_z
sum_global_x += global_x
sum_global_y += global_y
sum_global_z += global_z
# Coordinates of the centroids.
local_centroid_x = sum_local_x / num_fixed_points # x_s.
local_centroid_y = sum_local_y / num_fixed_points # y_s.
global_centroid_x = sum_global_x / num_fixed_points # X_s.
global_centroid_y = sum_global_y / num_fixed_points # Y_s.
# Calculate transformation parameters.
o_1 = o_2 = 0
a_1 = a_2 = 0
for name, fixed_point in self._fixed_points.items():
local_x = fixed_point.get('localX')
local_y = fixed_point.get('localY')
global_x = fixed_point.get('x')
global_y = fixed_point.get('y')
# Reduced coordinates of the centroids.
r_local_centroid_x = local_x - local_centroid_x
r_local_centroid_y = local_y - local_centroid_y
r_global_centroid_x = global_x - global_centroid_x
r_global_centroid_y = global_y - global_centroid_y
# o = [ x_i * Y_i - y_i * X_i ] * [ x_i^2 + y_i^2 ]^-1
o_1 += (r_local_centroid_x * r_global_centroid_y) -\
(r_local_centroid_y * r_global_centroid_x)
o_2 += math.pow(r_local_centroid_x, 2) +\
math.pow(r_local_centroid_y, 2)
# a = [ x_i * X_i + y_i * Y_i ] * [ x_i^2 + y_i^2 ]^-1
a_1 += (r_local_centroid_x * r_global_centroid_x) +\
(r_local_centroid_y * r_global_centroid_y)
a_2 += math.pow(r_local_centroid_x, 2) +\
math.pow(r_local_centroid_y, 2)
self._o = o_1 / o_2 if o_2 != 0 else 0 # Parameter o.
self._a = a_1 / a_2 if a_2 != 0 else 0 # Parameter a.
# Calculate the coordinates of the view point:
# Y_0 = Y_s - a * y_s - o * x_s
# X_0 = X_s - a * x_s + o * y_s
# Z_0 = ([Z] - [z]) / n
self._view_point['x'] = (global_centroid_x -
(self._a * local_centroid_x) +
(self._o * local_centroid_y))
self._view_point['y'] = (global_centroid_y -
(self._a * local_centroid_y) -
(self._o * local_centroid_x))
self._view_point['z'] = (sum_global_z - sum_local_z) / num_fixed_points
self.logger.info('Calculated coordinates of view point "{}" '
'(X = {:4.5f}, Y = {:4.5f}, Z = {:4.5f})'.format(
self._view_point.get('target'),
self._view_point.get('x'),
self._view_point.get('y'),
self._view_point.get('z')))
# Calculate the standard deviations.
sum_wx = sum_wy = 0 # [W_x], [W_y].
sum_wx_wx = sum_wy_wy = sum_wz_wz = 0 # [W_x^2], [W_y^2], [W_z^2].
for name, fixed_point in self._fixed_points.items():
local_x = fixed_point.get('localX')
local_y = fixed_point.get('localY')
local_z = fixed_point.get('localZ')
global_x = fixed_point.get('x')
global_y = fixed_point.get('y')
global_z = fixed_point.get('z')
view_point_x = self._view_point.get('x')
view_point_y = self._view_point.get('y')
view_point_z = self._view_point.get('z')
wx_i = ((-1 * view_point_x) - (self._a * local_x) +
(self._o * local_y) + global_x)
wy_i = ((-1 * view_point_y) - (self._a * local_y) -
(self._o * local_x) + global_y)
sum_wx += wx_i
sum_wy += wy_i
sum_wx_wx += wx_i * wx_i
sum_wy_wy += wy_i * wy_i
sum_wz_wz += math.pow(view_point_z - (global_z - local_z), 2)
# Sum of discrepancies should be 0, i.e., [W_x] = [W_y] = 0.
r_sum_wx = abs(round(sum_wx, 5))
r_sum_wy = abs(round(sum_wy, 5))
if r_sum_wx != 0 or r_sum_wy != 0:
self.logger.warning(f'Calculated coordinates of view point '
f'"{self._view_point.get("target")}" '
f'are inaccurate ([Wx] = {r_sum_wx}, '
f'[Wy] = {r_sum_wy})')
# Standard deviations.
sx = math.sqrt((sum_wx_wx + sum_wy_wy) / ((2 * num_fixed_points) - 4))
sy = sx
sz = math.sqrt(sum_wz_wz / (num_fixed_points - 1))
self.logger.debug(
'Calculated standard deviations '
'(sX = {:1.5f} m, sY = {:1.5f} m, sZ = {:1.5f} m)'.format(
sx, sy, sz))
# Scale factor.
m = math.sqrt((self._a * self._a) + (self._o * self._o))
self.logger.debug('Calculated scale factor (m = {:1.5f})'.format(m))
# Create response sets for the view point.
response_sets = {
'x': Obs.create_response_set('float', 'm', self._view_point['x']),
'y': Obs.create_response_set('float', 'm', self._view_point['y']),
'z': Obs.create_response_set('float', 'm', self._view_point['z']),
'stdDevX': Obs.create_response_set('float', 'm', sx),
'stdDevY': Obs.create_response_set('float', 'm', sy),
'stdDevZ': Obs.create_response_set('float', 'm', sz),
'scaleFactor': Obs.create_response_set('float', 'm', m)
}
# Create `Observation` instance for the view point.
view_point = Obs()
view_point.set('name', 'getViewPoint')
view_point.set('nextReceiver', 0)
view_point.set('nid', self._node_manager.node.id)
view_point.set('portName', obs.get('portName'))
view_point.set('pid', self._project_manager.project.id)
view_point.set('receivers', self._view_point.get('receivers'))
view_point.set('responseSets', response_sets)
view_point.set('sensorName', obs.get('sensorName'))
view_point.set('sensorType', obs.get('sensorType'))
view_point.set('target', self._view_point.get('target'))
view_point.set('timestamp', str(arrow.utcnow()))
# Return the `Observation` object of the view point.
return view_point
def _fire(self, c: int) -> None:
"""Creates a new observation with the given counter value.
Args:
c: The counter value.
"""
obs = Observation()
gpio = 'gpio{}'.format(self._pin)
response_sets = {
gpio: Observation.create_response_set('int', 'none', c)
}
obs.set('enabled', False)
obs.set('name', 'interruptCount')
obs.set('nextReceiver', 0)
obs.set('nid', self._node_manager.node.id)
obs.set('onetime', False)
obs.set('portName', f'GPIO{self._pin}')
obs.set('pid', self._project_manager.project.id)
obs.set('receivers', [self._receiver])
obs.set('responseSets', response_sets)
obs.set('sensorName', self._sensor_name)
obs.set('sensorType', 'gpio')
obs.set('sleepTime', 0.0)
obs.set('target', gpio)
obs.set('timestamp', str(arrow.utcnow()))
self.publish_observation(obs)