You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
98 lines
3.5 KiB
98 lines
3.5 KiB
############################################################################### |
|
# |
|
# Copyright (c) 2017 Estimation and Control Library (ECL). All rights reserved. |
|
# |
|
# Redistribution and use in source and binary forms, with or without |
|
# modification, are permitted provided that the following conditions |
|
# are met: |
|
# |
|
# 1. Redistributions of source code must retain the above copyright |
|
# notice, this list of conditions and the following disclaimer. |
|
# 2. Redistributions in binary form must reproduce the above copyright |
|
# notice, this list of conditions and the following disclaimer in |
|
# the documentation and/or other materials provided with the |
|
# distribution. |
|
# 3. Neither the name ECL nor the names of its contributors may be |
|
# used to endorse or promote products derived from this software |
|
# without specific prior written permission. |
|
# |
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
|
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
|
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
# POSSIBILITY OF SUCH DAMAGE. |
|
# |
|
############################################################################### |
|
|
|
"""Utils for the Python-based tests for the ecl library |
|
|
|
@author: Peter Dürr <Peter.Duerr@sony.com> |
|
""" |
|
from __future__ import print_function |
|
from __future__ import unicode_literals |
|
from __future__ import division |
|
from __future__ import absolute_import |
|
|
|
import numpy as np |
|
import pytest |
|
|
|
try: |
|
import ecl_EKF # pylint: disable=import-error |
|
except ImportError: |
|
print("ImportError: ecl library cannot be found." |
|
" Make sure to compile ecl with Python bindings " |
|
"(add -DPythonTests=1 to cmake invocation), " |
|
"and set the PYTHONPATH to your build directory.") |
|
exit(1) |
|
|
|
|
|
def float_array(inp): |
|
"""Convert input to an array of 32 bit floats |
|
""" |
|
return np.array(inp, dtype=np.float32) |
|
|
|
|
|
def update_sensors(ekf, # pylint: disable=too-many-arguments |
|
time_usec, |
|
dt_usec, |
|
accel=float_array([0.0, 0.0, -ecl_EKF.one_g]), |
|
ang_vel=float_array([0.0, 0.0, 0.0]), |
|
mag_data=float_array([1.0, 0.0, 0.0]), |
|
baro_data=0.0): |
|
"""Updates the sensors with inputs |
|
""" |
|
ekf.set_imu_data(time_usec, |
|
dt_usec, |
|
dt_usec, |
|
ang_vel * dt_usec / 1e6, |
|
accel * dt_usec / 1e6) |
|
ekf.set_mag_data(time_usec, |
|
mag_data) |
|
ekf.set_baro_data(time_usec, |
|
baro_data) |
|
|
|
|
|
@pytest.fixture |
|
def initialized_ekf(): |
|
"""Provides an initialized ekf, ready to go |
|
""" |
|
ekf = ecl_EKF.Ekf() |
|
|
|
time_usec = 1000 |
|
dt_usec = 5000 |
|
|
|
# Provide a few samples |
|
for _ in range(1000): |
|
update_sensors(ekf, time_usec, dt_usec) |
|
ekf.update() |
|
time_usec += dt_usec |
|
|
|
# Make sure the ekf updates as expected |
|
return ekf, time_usec
|
|
|