Source code for stonesoup.sensor.base

from abc import ABC
from typing import Optional

from stonesoup.movable import Movable, FixedMovable
from stonesoup.types.state import State
from stonesoup.types.angle import mod_bearing
from stonesoup.functions import cart2sphere, build_rotation_matrix_xyz

from ..base import Base, Property
from ..types.array import StateVector


[docs] class PlatformMountable(Base, ABC): """Base class for any object that can be mounted on a platform. All PlatformMountables must be mounted on a platform to calculate their position and orientation. To make this easier, if the sensor has a position and/or orientation specified in the constructor, and no :attr:`platform_system`, then the default is to create an internally held "private" platform for the Sensor. This allows the Sensor to control (and set) its own position and orientation. """ rotation_offset: StateVector = Property( default=None, doc="A StateVector containing the sensor rotation " "offsets from the platform's primary axis (defined as the " "direction of motion). Defaults to a zero vector with the " "same length as the Platform's :attr:`velocity_mapping`") mounting_offset: StateVector = Property( default=None, doc="A StateVector containing the sensor translation " "offsets from the platform's reference point. Defaults to " "a zero vector with length 3") movement_controller: Movable = Property( default=None, doc="The :class:`~.`Movable` object that controls the movement of this sensor. Will be " "set by the platform if the sensor is assigned to a platform." ) def __init__(self, *args, **kwargs): position = kwargs.pop('position', None) orientation = kwargs.pop('orientation', None) self.__has_internal_controller = False super().__init__(*args, **kwargs) if position is not None or orientation is not None: if position is None: # assuming 3d for a default platform position = StateVector([0, 0, 0]) if orientation is None: orientation = StateVector([0, 0, 0]) controller = FixedMovable( states=State(state_vector=position), position_mapping=list(range(len(position))), orientation=orientation) self._property_movement_controller = controller self.__has_internal_controller = True self._set_mounting_rotation_defaults() def _set_mounting_rotation_defaults(self): if self.movement_controller is None: return if self.mounting_offset is None: self.mounting_offset = StateVector([0] * len(self.movement_controller.position_mapping)) if self.rotation_offset is None: self.rotation_offset = StateVector([0] * 3) @movement_controller.setter def movement_controller(self, value): if self._has_internal_controller: raise AttributeError('The movement controller cannot be set on a Sensor with a private' 'internal controller') self._property_movement_controller = value self._set_mounting_rotation_defaults() @property def position(self) -> Optional[StateVector]: """The sensor position on a 3D Cartesian plane, expressed as a 3x1 :class:`StateVector` of Cartesian coordinates in the order :math:`x,y,z`. .. note:: This property delegates the actual calculation of position to the Sensor's :attr:`movement_controller` It is settable if, and only if, the sensor holds its own internal movement_controller. """ if self.movement_controller is None: return None return (self.movement_controller.position + self.movement_controller._get_rotated_offset(self.mounting_offset)) @position.setter def position(self, value: StateVector): if self._has_internal_controller: self.movement_controller.position = value else: raise AttributeError('Cannot set sensor position unless the sensor has its own ' 'default movement_controller') @property def orientation(self) -> Optional[StateVector]: """A 3x1 StateVector of angles (rad), specifying the sensor orientation in terms of the counter-clockwise rotation around each Cartesian axis in the order :math:`x,y,z`. The rotation angles are positive if the rotation is in the counter-clockwise direction when viewed by an observer looking along the respective rotation axis, towards the origin. .. note:: This property delegates the actual calculation of orientation to the Sensor's :attr:`movement_controller` It is settable if, and only if, the sensor holds its own internal movement_controller. """ if self.movement_controller is None: return None x_axis = (1, 0, 0) rotmat = (build_rotation_matrix_xyz(-self.movement_controller.orientation) @ build_rotation_matrix_xyz(-self.rotation_offset)) offset_axis = rotmat @ x_axis roll = mod_bearing(self.movement_controller.orientation[0] + self.rotation_offset[0]) # convert cartesian direction, 'offset_axis', into an orientation _, yaw, pitch = cart2sphere(*offset_axis) return StateVector([roll, pitch, yaw]) @orientation.setter def orientation(self, value: StateVector): if self._has_internal_controller: self.movement_controller.orientation = value else: raise AttributeError('Cannot set sensor position unless the sensor has its own ' 'default movement_controller') @property def _has_internal_controller(self): return self.__has_internal_controller @property def velocity(self) -> Optional[StateVector]: """The sensor velocity on a 3D Cartesian plane, expressed as a 3x1 :class:`StateVector` of Cartesian coordinates in the order :math:`x,y,z`. .. note:: This property delegates the actual calculation of velocity to the Sensor's :attr:`movement_controller` It is settable if, and only if, the sensor holds its own internal movement_controller which is a :class:`~.MovingMovable`.""" return self.movement_controller.velocity