Sub-modules
- cflib.localization.ippe_cf
- cflib.localization.lighthouse_bs_vector
- cflib.localization.lighthouse_cf_pose_sample
- cflib.localization.lighthouse_config_manager
- cflib.localization.lighthouse_geo_estimation_manager
- cflib.localization.lighthouse_geometry_solution
- cflib.localization.lighthouse_geometry_solver
- cflib.localization.lighthouse_initial_estimator
- cflib.localization.lighthouse_sweep_angle_reader
- cflib.localization.lighthouse_system_aligner
- cflib.localization.lighthouse_system_scaler
- cflib.localization.lighthouse_types
- cflib.localization.lighthouse_utils
- cflib.localization.param_io
- cflib.localization.user_action_detector
Classes
LhCfPoseSampleType
LhCfPoseSampleType(*args, **kwds)
An enum representing the type of a pose sample
Ancestors (in MRO)
- enum.Enum
Class variables
ORIGIN
VERIFICATION
XYZ_SPACE
XY_PLANE
X_AXIS
LhDeck4SensorPositions
LhDeck4SensorPositions()
Positions of the sensors on the Lighthouse 4 deck
Class variables
diagonal_distance
positions
LighthouseBsVector
LighthouseBsVector(lh_v1_horiz_angle: float, lh_v1_vert_angle: float)
This class is representing a vector from a base station into space, in the base station reference frame. Typically the intersection of two light planes defined by angles measured by a base station. It also provides functionality to convert between lighthouse V1 angles, V2 angles and cartesian coordinates.
Initialize from lighthouse V1 angles
Parameters
| Name | Description |
|---|---|
| lh_v1_horiz_angle | Horizontal sweep angle, 0 straight forward. Right (seen from the bs) is negative, left is positive |
| lh_v1_vert_angle | Vertical sweep angle, 0 straight forward. Down is negative, up is positive. |
Class variables
T
Static methods
def from_cart(cart_vector: list[float]) ‑> cflib.localization.lighthouse_bs_vector.LighthouseBsVector
Create a LighthouseBsVector object from cartesian coordinates.
Parameters
| Name | Description |
|---|---|
| cart_vector | (x, y, z) to a point |
def from_lh2(lh_v2_angle_1: float, lh_v2_angle_2: float) ‑> cflib.localization.lighthouse_bs_vector.LighthouseBsVector
Create a LighthouseBsVector object from lighthouse V2 angles
Parameters
| Name | Description |
|---|---|
| lh_v2_angle_1 | First sweep angles, 0 straight ahead |
| lh_v2_angle_2 | Second sweep angles, 0 straight ahead |
def from_projection(proj_point: list[float]) ‑> cflib.localization.lighthouse_bs_vector.LighthouseBsVector
Create a LighthouseBsVector object from the projection point on the plane x=1.0
Parameters
| Name | Description |
|---|---|
| projection point | (y, z) |
def yaml_constructor(loader, node)
def yaml_representer(dumper, data: "'LighthouseBsVector'")
Instance variables
cart: numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[numpy.float32]]
A normalized vector in cartesian coordinates
lh_v1_angle_pair: tuple[float, float]
Lightouse V1 angle pair (horiz, vert)
lh_v1_horiz_angle: float
Lightouse V1 horizontal sweep angle
lh_v1_vert_angle: float
Lightouse V1 vertical sweep angle
lh_v2_angle_1: float
Lightouse V2 first sweep angle
lh_v2_angle_2: float
Lightouse V2 second sweep angle
projection: numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[numpy.float32]]
The 2D point (y, z) when projected on the plane x=1.0 (one meter in front of the base station)
LighthouseConfigFileManager
LighthouseConfigFileManager()
Class variables
CALIBS_ID
GEOS_ID
SYSTEM_TYPE_ID
SYSTEM_TYPE_V1
SYSTEM_TYPE_V2
TYPE
TYPE_ID
VERSION
VERSION_ID
Static methods
def read(file_name)
def write(file_name, geos={}, calibs={}, system_type=2)
LighthouseConfigWriter
LighthouseConfigWriter(cf, nr_of_base_stations=16)
This class is used to write system config data to the Crazyflie RAM and persis to permanent storage
Instance variables
is_write_ongoing: bool
Methods
def write_and_store_config(self, data_stored_cb, geos=None, calibs=None, system_type=None)
Transfer geometry and calibration data to the Crazyflie and persist to permanent storage. The callback is called when done. If geos or calibs is None, no data will be written for that data type. If geos or calibs is a dictionary, the values for the base stations in the dictionary will transferred to the Crazyflie, data for all other base stations will be invalidated.
def write_and_store_config_from_file(self, data_stored_cb, file_name)
Read system configuration data from file and write/persist to the Crazyflie. Geometry and calibration data for base stations that are not in the config file will be invalidated.
LighthouseCrossingBeam
LighthouseCrossingBeam()
A class to calculate the crossing point of two “beams” from two base stations. The beams are defined by the line where the two light planes intersect. In a perfect world the crossing point of the two beams is the position of a sensor on the Crazyflie Lighthouse deck, but in reality the beams will most likely not cross and instead we use the closest point between the two beams as the position estimate. The (minimum) distance between the beams is also calculated and can be used as an error estimate for the position.
Static methods
def distance_sensor(bs1: Pose, angles_bs1: LighthouseBsVector, bs2: Pose, angles_bs2: LighthouseBsVector) ‑> float
Calculate the minimum distance between the beams from two base stations.
Args: bs1 (Pose): The pose of the first base station. angles_bs1 (LighthouseBsVector): The sweep angles of the first base station. bs2 (Pose): The pose of the second base station. angles_bs2 (LighthouseBsVector): The sweep angles of the second base station.
Returns: float: The shortest distance between the beams.
def max_distance_all_permutations(bs_angles: list[tuple[Pose, LighthouseBsVectors]]) ‑> float
Calculate the maximum distance between the beams from base stations for all sensors. All permutations of base stations are considered. This result can be used as an estimation of the maximum error.
Args: bs_angles (list[tuple[Pose, LighthouseBsVectors]]): A list of tuples containing the pose of the base stations and their sweep angles.
Returns: float: The maximum distance between the beams from all permutations of base stations.
def position_distance_sensor(bs1: Pose, angles_bs1: LighthouseBsVector, bs2: Pose, angles_bs2: LighthouseBsVector) ‑> tuple[numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]], float]
Calculate the estimated position of the crossing point of the beams from two base stations as well as the distance.
Args: bs1 (Pose): The pose of the first base station. angles_bs1 (LighthouseBsVector): The sweep angles of the first base station. bs2 (Pose): The pose of the second base station. angles_bs2 (LighthouseBsVector): The sweep angles of the second base station.
Returns: tuple[npt.NDArray, float]: The estimated position of the crossing point and the distance between the beams.
def position_max_distance(bs1: Pose, angles_bs1: LighthouseBsVectors, bs2: Pose, angles_bs2: LighthouseBsVectors) ‑> tuple[numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]], float]
Calculate the position and maximum distance between the beams from two base stations. The position is the average position for all sensors which is the center of the lighthouse deck.
Args: bs1 (Pose): The pose of the first base station. angles_bs1 (LighthouseBsVectors): The sweep angles of the first base station. bs2 (Pose): The pose of the second base station. angles_bs2 (LighthouseBsVectors): The sweep angles of the second base station.
Returns: float: The position and maximum distance between the beams.
def position_max_distance_all_permutations(bs_angles: list[tuple[Pose, LighthouseBsVectors]]) ‑> tuple[numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]], float]
Calculate the average position and the maximum distance between the beams from base stations for all sensors. All permutations of base stations are considered.
The position will be an estimate of the position of the center of the lighthouse deck and the maximum distance can be used as an estimation of the maximum error.
Args: bs_angles (list[tuple[Pose, LighthouseBsVectors]]): A list of tuples containing the pose of the base stations and their sweep angles.
Returns: tuple[npt.NDArray, float]: The position and the maximum distance between the beams from all permutations of base stations.
def position_sensor(bs1: Pose, angles_bs1: LighthouseBsVector, bs2: Pose, angles_bs2: LighthouseBsVector) ‑> numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]
Calculate the estimated position of the crossing point of the beams from two base stations.
Args: bs1 (Pose): The pose of the first base station. angles_bs1 (LighthouseBsVector): The sweep angles of the first base station. bs2 (Pose): The pose of the second base station. angles_bs2 (LighthouseBsVector): The sweep angles of the second base station.
Returns: npt.NDArray: The estimated position of the crossing point of the two beams.
def positions_distances(bs1: Pose, angles_bs1: LighthouseBsVectors, bs2: Pose, angles_bs2: LighthouseBsVectors) ‑> list[tuple[numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]], float]]
Calculate the positions and minimum distance between the beams from two base stations for all sensors.
Args: bs1 (Pose): The pose of the first base station. angles_bs1 (LighthouseBsVectors): The sweep angles of the first base station. bs2 (Pose): The pose of the second base station. angles_bs2 (LighthouseBsVectors): The sweep angles of the second base station.
Returns: list[tuple[npt.NDArray, float]]: A list of the positions and distances for each sensor.
LighthouseGeometrySolution
LighthouseGeometrySolution(samples: list[cflib.localization.lighthouse_cf_pose_sample.LhCfPoseSampleWrapper])
A class to represent the solution of a lighthouse geometry problem.
Class variables
ErrorStats
LighthouseMatchedSweepAngleReader
LighthouseMatchedSweepAngleReader(cf: cflib.crazyflie.Crazyflie, data_recevied_cb, timeout_cb=None, sample_count: int = 1, min_bs: int = 2, max_time_ms: int = 25)
Wrapper to simplify reading of matched lighthouse sweep angles from the locSrv stream
Class variables
MATCHED_STREAM_MAX_TIME_PARAM
MATCHED_STREAM_MIN_BS_PARAM
MATCHED_STREAM_PARAM
NR_OF_SENSORS
Methods
def start(self, timeout: float = 0.0)
Start reading sweep angles
Args: timeout (float): timeout in seconds, 0.0 means no timeout
def stop(self)
Stop reading sweep angles
LighthouseSweepAngleAverageReader
LighthouseSweepAngleAverageReader(cf: cflib.crazyflie.Crazyflie, ready_cb: collections.abc.Callable[[dict[int, tuple[int, cflib.localization.lighthouse_bs_vector.LighthouseBsVectors]]], None])
Helper class to make it easy read sweep angles for multiple base stations and average the result
Methods
def is_collecting(self)
True if data collection is in progress
def start_angle_collection(self)
Start collecting angles. The process will terminate when nr_of_samples_required have been received
def stop_angle_collection(self)
Premature stop of data collection
LighthouseSweepAngleReader
LighthouseSweepAngleReader(cf: cflib.crazyflie.Crazyflie, data_recevied_cb)
Wrapper to simplify reading of lighthouse sweep angles from the locSrv stream
Class variables
ANGLE_STREAM_PARAM
NR_OF_SENSORS
Methods
def start(self)
Start reading sweep angles
def stop(self)
Stop reading sweep angles
ParamFileManager
ParamFileManager()
Reads and writes parameter configurations from file
Class variables
PARAMS_ID
TYPE
TYPE_ID
VERSION
VERSION_ID
Static methods
def read(file_name)
def write(file_name, params={})
Pose
Pose(R_matrix: npt.ArrayLike = array([[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.]]), t_vec: npt.ArrayLike = array([0., 0., 0.]))
Holds the full pose (position and orientation) of an object. Contains functionality to convert between various formats.
Static methods
def from_cf_rpy(roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0, t_vec: npt.ArrayLike = array([0., 0., 0.])) ‑> cflib.localization.lighthouse_types.Pose
Create a Pose from roll, pitch and yaw angles in the Crazyflie convention and translation vector
Args: roll (float, optional): Roll angle as used in the CF (degrees). Defaults to 0.0. pitch (float, optional): Pitch angle as used in the CF (degrees). Defaults to 0.0. yaw (float, optional): Yaw angle as used in the CF (degrees). Defaults to 0.0. t_vec (npt.ArrayLike, optional): Position vector. Defaults to _ORIGIN.
Returns: Pose: The created Pose object
def from_quat(R_quat: npt.ArrayLike = array([0., 0., 0., 0.]), t_vec: npt.ArrayLike = array([0., 0., 0.])) ‑> cflib.localization.lighthouse_types.Pose
Create a Pose from a quaternion and translation vector
def from_rot_vec(R_vec: npt.ArrayLike = array([0., 0., 0.]), t_vec: npt.ArrayLike = array([0., 0., 0.])) ‑> cflib.localization.lighthouse_types.Pose
Create a Pose from a rotation vector and translation vector
def from_rpy(roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0, t_vec: npt.ArrayLike = array([0., 0., 0.]), seq: str = 'xyz', degrees: bool = False) ‑> cflib.localization.lighthouse_types.Pose
Create a Pose from roll, pitch and yaw angles and translation vector
Args: roll (float, optional): Roll angle. Defaults to 0.0. pitch (float, optional): _Pitch angle. Defaults to 0.0. yaw (float, optional): Yaw angle. Defaults to 0.0. t_vec (npt.ArrayLike, optional): Position vector. Defaults to _ORIGIN. seq (str, optional): The order of roll, pitch and yaw, see scipy documentation for Rotation.from_euler. degrees (bool, optional): Whether the angles are in degrees. Defaults to False.
Returns: Pose: The created Pose object
def yaml_constructor(loader, node)
Construct a Pose object from YAML
def yaml_representer(dumper, data: Pose)
Represent a Pose object in YAML
Instance variables
matrix_vec: tuple[numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]], numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]]
Get the pose as a rotation matrix and translation vector
rot_cf_rpy: tuple[float, float, float]
Get roll, pitch and yaw of the pose in the Crazyflie convention (degrees)
Returns: tuple[float, float, float]: roll, pitch, yaw in degrees as used in the CF
rot_matrix: numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]
Get the rotation matrix of the pose
rot_quat: numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]
Get the quaternion of the pose
rot_vec: numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]
Get the rotation vector of the pose
translation: numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]
Get the translation vector of the pose
Methods
def inv_rotate_translate(self, point: npt.ArrayLike) ‑> numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]
Inverse rotate and translate a point, that is transform from global to local reference frame
def inv_rotate_translate_pose(self, pose: "'Pose'") ‑> cflib.localization.lighthouse_types.Pose
Inverse rotate and translate a point, that is transform from global to local reference frame
def rot_euler(self, seq: str = 'xyz', degrees: bool = False) ‑> numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]
Get the euler angles of the pose
Args: seq (str, optional): The order of roll, pitch and yaw, see scipy documentation for Rotation.as_euler. use ‘xyz’ for the Crazyflie convention (default). degrees (bool, optional): Whether to return the angles in degrees. Defaults to False.
Returns: npt.NDArray: The euler angles of the pose
def rotate_translate(self, point: npt.ArrayLike) ‑> numpy.ndarray[tuple[typing.Any, ...], numpy.dtype[~_ScalarT]]
Rotate and translate a point, that is transform from local to global reference frame
def rotate_translate_pose(self, pose: "'Pose'") ‑> cflib.localization.lighthouse_types.Pose
Rotate and translate a pose
def scale(self, scale) ‑> NoneType
quiet