18.5.0
Abstract
This pull-request is a part of #1577 by @TauTheLepton
In this pull-request, the noise for /sensing/imu/imu_data topic is introduced.
Background
Details
The following folding document is copied from #1577 and are closely related to this pull request:
Simple sensor simulator noise
### Simple sensor simulator noise Noise adding framework from `concealer::Publisher` has been used implementing simple sensor simulator noise, that is the noise applied to topic - `/sensing/imu/imu_data`Applying noise to this topic has been slightly more complicated, because the sensor already had some noise implementation.
The older noise implementation is incompatible with the normal distribution noise framework fromconcealer::Publisher.
For this reason, and the fact we want to keep the simulator backward compatible - the new noise adding method has been added "on top" of the old implementation with a switch enabling to choose which implementation should be used.
This means the original implementation is used by default and only when user specifies the parameter /sensing/imu/imu_data.override_legacy_configuration to be true the legacy method is switched off and the new method is switched on (part of this process involves calculating new covariance matrices, because they depend on the new noise parameters).
Additionally, unit tests (testing only the noise application correctness) for ImuSensor with both noise configurations (legacy and new) have been implemented.
Additional development from #1577
Add configurable noises for diagnal elements of covariance matrices
In #1577, the diagnosial elements of covariance matrices were implemented in a way that linked to the noise settings of the corresponding elements of the IMU message, but by request they have now been implemented as independent noise.
configuration switch
The override_legacy_configuration parameter controls which noise settings are used:
- false (default): Use legacy noise settings from
ImuSensorConfiguration(Protobuf), which are passed viaattachImuSensorAPI. These settings includenoise_standard_deviation_orientation,noise_standard_deviation_twist,
andnoise_standard_deviation_acceleration. The ROS parameters in this file will be IGNORED. - true: Use the following ROS parameters for detailed noise configuration. Legacy settings from
ImuSensorConfigurationwill be IGNORED.
parameters
You can set the additive/multiplicative errors with specifying mean/standard_deviation parameters for orientation, angular_velocity, linear_acceleration and their covariances.
The orientation parameters are r, p and y (Euler angles), and the angular_velocity / linear_acceleration parameters are x, y and z.
The covariance parameters are diagonal elements only: roll_roll, pitch_pitch, yaw_yaw for orientation covariance, and x_x, y_y, z_z for angular_velocity / linear_acceleration covariances.
Details
/sensing/imu/imu_data:
version: 20240605 # architecture_type suffix (mandatory)
override_legacy_configuration: false # This parameter (if true) disables legacy configuration and overrides it with the following parameters
# The following parameters are used only if `override_legacy_configuration` is true.
seed: 0 # If 0 is specified, a random seed value will be generated for each run.
sensor_msgs::msg::Imu:
orientation:
# The type of sensor_msgs::msg::Imu.orientation is Quaternion,
# and the actual orientation data members are x, y, z, and w.
# However, applying error to Quaternions can be unintuitive and
# tricky, so we accept the parameters as Euler angles here. The
# simulator internally converts Quaternion to Euler angles and
# applies the error to them. It then converts the error-applied
# Euler angles back to Quaternion and publishes them as
# `/sensing/imu/imu_data`.
r:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
p:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
y:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
angular_velocity:
x:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
y:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
z:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
linear_acceleration:
x:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
y:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
z:
error:
additive:
mean: 0.0
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
orientation_covariance:
roll_roll:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
pitch_pitch:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
yaw_yaw:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
angular_velocity_covariance:
x_x:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
y_y:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
z_z:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
linear_acceleration_covariance:
x_x:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
y_y:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0
z_z:
error:
additive:
mean: 0.0001
standard_deviation: 0.0
multiplicative:
mean: 0.0
standard_deviation: 0.0References
Regression Test: OK
Destructive Changes
None
The default parameter is zero.
So the default behavior is not changed
