Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Sensors from JSON and more #2821

Closed
wants to merge 8 commits into from
Closed

Sensors from JSON and more #2821

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
00f366f
added DistanceSensor, fixed shake
ACLeighner Jun 19, 2020
f6d169f
Added the remaining sensors to be populated by settings.json
ACLeighner Jun 23, 2020
5402063
autonomy python files
ACLeighner Jan 12, 2021
41a8698
Add files via upload
ACLeighner Feb 9, 2021
bcd3e0a
Add files via upload
ACLeighner Feb 22, 2021
e1b13e8
Add files via upload
ACLeighner Apr 7, 2021
aa7e96c
Add files via upload
ACLeighner Apr 7, 2021
a4852eb
Add files via upload
ACLeighner Apr 9, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
166 changes: 147 additions & 19 deletions AirLib/include/common/AirSimSettings.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -184,18 +184,84 @@ struct AirSimSettings {
};

struct BarometerSetting : SensorSetting {
real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa
real_T pressure_factor_sigma = 0.0365f / 20;
real_T pressure_factor_tau = 3600;
real_T unnorrelated_noise_sigma = 0.027f * 100;
real_T update_latency = 0.0f; //sec
real_T update_frequency = 50; //Hz
real_T startup_delay = 0; //sec
Vector3r position = VectorMath::nanVector();
Rotation rotation = Rotation::nanRotation();
};

struct ImuSetting : SensorSetting {
//angule random walk (ARW)
real_T gyro_arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec)
//Bias Stability (tau = 500s)
real_T gyro_tau = 500;
real_T gyro_bias_stability = 4.6f / 3600 * M_PIf / 180; //deg/hr converted to rad/sec
Vector3r gyro_turn_on_bias = Vector3r::Zero(); //assume calibration is done

//velocity random walk (ARW)
real_T accel_vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2
//Bias Stability (tau = 800s)
real_T accel_tau = 800;
real_T accel_bias_stability = 36.0f * 1E-6f * 9.80665f; //ug converted to m/s^2
Vector3r accel_turn_on_bias = Vector3r::Zero(); //assume calibration is done
Vector3r position = VectorMath::nanVector();
Rotation rotation = Rotation::nanRotation();

};

struct GpsSetting : SensorSetting {

real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f;
real_T eph_initial = 100.0f, epv_initial = 100.0f; //initially fully diluted positions
real_T eph_final = 0.3f, epv_final = 0.4f;
real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f;

real_T update_latency = 0.2f; //sec
real_T update_frequency = 50; //Hz
real_T startup_delay = 1; //sec

Vector3r position = VectorMath::nanVector();
Rotation rotation = Rotation::nanRotation();

};

struct MagnetometerSetting : SensorSetting {
Vector3r noise_sigma = Vector3r(0.005f, 0.005f, 0.005f); //5 mgauss as per specs sheet (RMS is same as stddev) https://goo.gl/UOz6FT
real_T scale_factor = 1.0f;
Vector3r noise_bias = Vector3r(0.0f, 0.0f, 0.0f); //no offset as per specsheet (zero gauss level) https://goo.gl/UOz6FT
float ref_update_frequency = 0.2f; //Hz

//use dipole model if there is enough compute power available
bool dynamic_reference_source = true;
// ref_source 0==ConstantModel; 1==DipoleModel
int ref_source = 1;

//see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
real_T update_latency = 0.0f; //sec: from PX4 doc
real_T update_frequency = 50; //Hz
real_T startup_delay = 0; //sec

Vector3r position = VectorMath::nanVector();
Rotation rotation = Rotation::nanRotation();

};

struct DistanceSetting : SensorSetting {
//nan means keep the default values set in components
Vector3r position = VectorMath::nanVector();
Rotation rotation = Rotation::nanRotation();

real_T min_distance = 20.0f / 100; //m
real_T max_distance = 4000.0f / 100; //m
real_T unnorrelated_noise_sigma = 0.002f * 100;
real_T update_latency = 0.0f; //sec
real_T update_frequency = 50; //Hz
real_T startup_delay = 0; //sec
};

struct LidarSetting : SensorSetting {
Expand Down Expand Up @@ -694,13 +760,42 @@ struct AirSimSettings {
settings_json.getFloat("Y", default_vec.y()),
settings_json.getFloat("Z", default_vec.z()));
}

static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot)
{
return Rotation(settings_json.getFloat("Yaw", default_rot.yaw),
settings_json.getFloat("Pitch", default_rot.pitch),
settings_json.getFloat("Roll", default_rot.roll));
}

static Vector3r createGyroBiasSetting(const Settings& settings_json, const Vector3r& default_vec)
{
return Vector3r(settings_json.getFloat("GyroBias_X", default_vec.x()),
settings_json.getFloat("GyroBias_Y", default_vec.y()),
settings_json.getFloat("GyroBias_Z", default_vec.z()));
}

static Vector3r createAccelBiasSetting(const Settings& settings_json, const Vector3r& default_vec)
{
return Vector3r(settings_json.getFloat("AccelBias_X", default_vec.x()),
settings_json.getFloat("AccelBias_Y", default_vec.y()),
settings_json.getFloat("AccelBias_Z", default_vec.z()));
}

static Vector3r createNoiseSigmaSetting(const Settings& settings_json, const Vector3r& default_vec)
{
return Vector3r(settings_json.getFloat("NoiseSigma_X", default_vec.x()),
settings_json.getFloat("NoiseSigma_Y", default_vec.y()),
settings_json.getFloat("NoiseSigma_Z", default_vec.z()));
}

static Vector3r createNoiseBiasSetting(const Settings& settings_json, const Vector3r& default_vec)
{
return Vector3r(settings_json.getFloat("NoiseBias_X", default_vec.x()),
settings_json.getFloat("NoiseBias_Y", default_vec.y()),
settings_json.getFloat("NoiseBias_Z", default_vec.z()));
}

static std::unique_ptr<VehicleSetting> createVehicleSetting(const std::string& simmode_name, const Settings& settings_json,
const std::string vehicle_name)
{
Expand Down Expand Up @@ -1122,42 +1217,75 @@ struct AirSimSettings {

static void initializeBarometerSetting(BarometerSetting& barometer_setting, const Settings& settings_json)
{
unused(barometer_setting);
unused(settings_json);

//TODO: set from json as needed
barometer_setting.qnh = settings_json.getFloat("SeaLevelPressure", barometer_setting.qnh);
barometer_setting.pressure_factor_sigma = settings_json.getFloat("PressureFactorSigma", barometer_setting.pressure_factor_sigma);
barometer_setting.pressure_factor_tau = settings_json.getFloat("PressureFactorTau", barometer_setting.pressure_factor_tau);
barometer_setting.unnorrelated_noise_sigma = settings_json.getFloat("NoiseSigma", barometer_setting.unnorrelated_noise_sigma);
barometer_setting.update_latency = settings_json.getFloat("UpdateLatency", barometer_setting.update_latency);
barometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", barometer_setting.update_frequency);
barometer_setting.startup_delay = settings_json.getFloat("StartupDelay", barometer_setting.startup_delay);
barometer_setting.position = createVectorSetting(settings_json, barometer_setting.position);
barometer_setting.rotation = createRotationSetting(settings_json, barometer_setting.rotation);
}

static void initializeImuSetting(ImuSetting& imu_setting, const Settings& settings_json)
{
unused(imu_setting);
unused(settings_json);

//TODO: set from json as needed
imu_setting.gyro_arw = settings_json.getFloat("GyroAngleRandomWalk", imu_setting.gyro_arw);
imu_setting.gyro_tau = settings_json.getFloat("GyroBiasTau", imu_setting.gyro_tau);
imu_setting.gyro_bias_stability = settings_json.getFloat("GyroBiasStability", imu_setting.gyro_bias_stability);
imu_setting.gyro_turn_on_bias = createGyroBiasSetting(settings_json, imu_setting.gyro_turn_on_bias);

imu_setting.accel_vrw = settings_json.getFloat("AccelerometerVelocityRandomWalk", imu_setting.accel_vrw);
imu_setting.accel_tau = settings_json.getFloat("AccelerometerBiasTau", imu_setting.accel_tau);
imu_setting.accel_bias_stability = settings_json.getFloat("AccelerometerBiasStability", imu_setting.accel_bias_stability);
imu_setting.accel_turn_on_bias = createAccelBiasSetting(settings_json, imu_setting.accel_turn_on_bias);

imu_setting.position = createVectorSetting(settings_json, imu_setting.position);
imu_setting.rotation = createRotationSetting(settings_json, imu_setting.rotation);
}

static void initializeGpsSetting(GpsSetting& gps_setting, const Settings& settings_json)
{
unused(gps_setting);
unused(settings_json);
gps_setting.eph_time_constant = settings_json.getFloat("EphTimeConstant", gps_setting.eph_time_constant);
gps_setting.epv_time_constant = settings_json.getFloat("EpvTimeConstant", gps_setting.epv_time_constant);
gps_setting.eph_initial = settings_json.getFloat("EphInitial", gps_setting.eph_initial);
gps_setting.epv_initial = settings_json.getFloat("EpvInitial", gps_setting.epv_initial);
gps_setting.eph_final = settings_json.getFloat("EphFinal", gps_setting.eph_final);
gps_setting.epv_final = settings_json.getFloat("EpvFinal", gps_setting.epv_final);
gps_setting.eph_min_3d = settings_json.getFloat("EphMin3d", gps_setting.eph_min_3d);
gps_setting.eph_min_2d = settings_json.getFloat("EphMin2d", gps_setting.eph_min_2d);
gps_setting.update_latency = settings_json.getFloat("UpdateLatency", gps_setting.update_latency);
gps_setting.update_frequency = settings_json.getFloat("UpdateFrequency", gps_setting.update_frequency);
gps_setting.startup_delay = settings_json.getFloat("StartupDelay", gps_setting.startup_delay);
gps_setting.position = createVectorSetting(settings_json, gps_setting.position);
gps_setting.rotation = createRotationSetting(settings_json, gps_setting.rotation);

//TODO: set from json as needed
}

static void initializeMagnetometerSetting(MagnetometerSetting& magnetometer_setting, const Settings& settings_json)
{
unused(magnetometer_setting);
unused(settings_json);

//TODO: set from json as needed
magnetometer_setting.noise_sigma = createNoiseSigmaSetting(settings_json, magnetometer_setting.noise_sigma);
magnetometer_setting.scale_factor = settings_json.getFloat("ScaleFactor", magnetometer_setting.scale_factor);
magnetometer_setting.dynamic_reference_source = settings_json.getBool("DynamicReferenceSource", magnetometer_setting.dynamic_reference_source);
magnetometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", magnetometer_setting.update_frequency);
magnetometer_setting.ref_source = settings_json.getInt("ReferenceSource", magnetometer_setting.ref_source);
magnetometer_setting.update_latency = settings_json.getFloat("UpdateLatency", magnetometer_setting.update_latency);
magnetometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", magnetometer_setting.update_frequency);
magnetometer_setting.startup_delay = settings_json.getFloat("StartupDelay", magnetometer_setting.startup_delay);
magnetometer_setting.position = createVectorSetting(settings_json, magnetometer_setting.position);
magnetometer_setting.rotation = createRotationSetting(settings_json, magnetometer_setting.rotation);
}

static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& settings_json)
{
unused(distance_setting);
unused(settings_json);

//TODO: set from json as needed
distance_setting.min_distance = settings_json.getFloat("MinDistance", distance_setting.min_distance);
distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance);
distance_setting.unnorrelated_noise_sigma = settings_json.getFloat("Noise", distance_setting.unnorrelated_noise_sigma);
distance_setting.update_latency = settings_json.getFloat("UpdateLatency", distance_setting.update_latency);
distance_setting.update_frequency = settings_json.getFloat("UpdateFrequency", distance_setting.update_frequency);
distance_setting.startup_delay = settings_json.getFloat("StartupDelay", distance_setting.startup_delay);
distance_setting.position = createVectorSetting(settings_json, distance_setting.position);
distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation);
}

static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json)
Expand Down
27 changes: 26 additions & 1 deletion AirLib/include/sensors/barometer/BarometerSimpleParams.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,34 @@ struct BarometerSimpleParams {
real_T update_frequency = 50; //Hz
real_T startup_delay = 0; //sec

Pose relative_pose;

void initializeFromSettings(const AirSimSettings::BarometerSetting& settings)
{
unused(settings);
qnh = settings.qnh;
pressure_factor_sigma = settings.pressure_factor_sigma;
pressure_factor_tau = settings.pressure_factor_tau;
unnorrelated_noise_sigma = settings.unnorrelated_noise_sigma;
update_latency = settings.update_latency;
update_frequency = settings.update_frequency;
startup_delay = settings.startup_delay;

relative_pose.position = settings.position;
if (std::isnan(relative_pose.position.x()))
relative_pose.position.x() = 0;
if (std::isnan(relative_pose.position.y()))
relative_pose.position.y() = 0;
if (std::isnan(relative_pose.position.z()))
relative_pose.position.z() = 0;

float pitch, roll, yaw;
pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0;
roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0;
yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0;
relative_pose.orientation = VectorMath::toQuaternion(
Utils::degreesToRadians(pitch), //pitch - rotation around Y axis
Utils::degreesToRadians(roll), //roll - rotation around X axis
Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis
}
};

Expand Down
24 changes: 23 additions & 1 deletion AirLib/include/sensors/distance/DistanceSimpleParams.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,29 @@ struct DistanceSimpleParams {

void initializeFromSettings(const AirSimSettings::DistanceSetting& settings)
{
unused(settings);
min_distance = settings.min_distance;
max_distance = settings.max_distance;
unnorrelated_noise_sigma = settings.unnorrelated_noise_sigma;
update_latency = settings.update_latency;
update_frequency = settings.update_frequency;
startup_delay = settings.startup_delay;

relative_pose.position = settings.position;
if (std::isnan(relative_pose.position.x()))
relative_pose.position.x() = 0;
if (std::isnan(relative_pose.position.y()))
relative_pose.position.y() = 0;
if (std::isnan(relative_pose.position.z()))
relative_pose.position.z() = 0;

float pitch, roll, yaw;
pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0;
roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0;
yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0;
relative_pose.orientation = VectorMath::toQuaternion(
Utils::degreesToRadians(pitch), //pitch - rotation around Y axis
Utils::degreesToRadians(roll), //roll - rotation around X axis
Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis
}
};

Expand Down
34 changes: 32 additions & 2 deletions AirLib/include/sensors/gps/GpsSimpleParams.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
#define msr_airlib_GpsSimpleParams_hpp

#include "common/Common.hpp"

#include "common/AirSimSettings.hpp"

namespace msr { namespace airlib {

Expand All @@ -19,9 +19,39 @@ struct GpsSimpleParams {
real_T update_frequency = 50; //Hz
real_T startup_delay = 1; //sec

Pose relative_pose;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Unused


void initializeFromSettings(const AirSimSettings::GpsSetting& settings)
{
unused(settings);
eph_time_constant = settings.eph_time_constant;
epv_time_constant = settings.epv_time_constant;
eph_initial = settings.eph_initial;
epv_initial = settings.epv_initial;
eph_final = settings.eph_final;
epv_final = settings.epv_final;
eph_min_3d = settings.eph_min_3d;
eph_min_2d = settings.eph_min_2d;

update_latency = settings.update_latency;
update_frequency = settings.update_frequency;
startup_delay = settings.startup_delay;

relative_pose.position = settings.position;
if (std::isnan(relative_pose.position.x()))
relative_pose.position.x() = 0;
if (std::isnan(relative_pose.position.y()))
relative_pose.position.y() = 0;
if (std::isnan(relative_pose.position.z()))
relative_pose.position.z() = 0;

float pitch, roll, yaw;
pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0;
roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0;
yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0;
relative_pose.orientation = VectorMath::toQuaternion(
Utils::degreesToRadians(pitch), //pitch - rotation around Y axis
Utils::degreesToRadians(roll), //roll - rotation around X axis
Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis
}
};

Expand Down
31 changes: 29 additions & 2 deletions AirLib/include/sensors/imu/ImuSimpleParams.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,8 @@ struct ImuSimpleParams {
https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
For Allan Variance/Deviation plots see http://www.invensense.com/wp-content/uploads/2015/02/MPU-3300-Datasheet.pdf
*/
Pose relative_pose;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't see this being used anywhere?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes, although relative pose is being consumed in the initialization of settings, it is not incorporated into the sensor model.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ACLeighner can you either remove this setting or make the corresponding changes in the sensor model. (My guess would be you were in the middle of doing the same?)


struct Gyroscope {
//angule random walk (ARW)
real_T arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec)
Expand All @@ -43,10 +45,35 @@ struct ImuSimpleParams {
} accel;

real_T min_sample_time = 1 / 1000.0f; //internal IMU frequency

void initializeFromSettings(const AirSimSettings::ImuSetting& settings)
{
unused(settings);
gyro.arw = settings.gyro_arw;
gyro.tau = settings.gyro_tau;
gyro.bias_stability = settings.gyro_bias_stability;
gyro.turn_on_bias = settings.gyro_turn_on_bias;

accel.vrw = settings.accel_vrw;
accel.tau = settings.accel_tau;
accel.bias_stability = settings.accel_bias_stability;
accel.turn_on_bias = settings.accel_turn_on_bias;

relative_pose.position = settings.position;
if (std::isnan(relative_pose.position.x()))
relative_pose.position.x() = 0;
if (std::isnan(relative_pose.position.y()))
relative_pose.position.y() = 0;
if (std::isnan(relative_pose.position.z()))
relative_pose.position.z() = 0;

float pitch, roll, yaw;
pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0;
roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0;
yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0;
relative_pose.orientation = VectorMath::toQuaternion(
Utils::degreesToRadians(pitch), //pitch - rotation around Y axis
Utils::degreesToRadians(roll), //roll - rotation around X axis
Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis
}
};

Expand Down
Loading