Add noise for pose

[HansRobo]

Abstract

This pull-request is a part of #1577 by @TauTheLepton

In this pull-request, the noise for /localization/pose_estimator/pose_with_covariance topic is introduced.

Background

Details

The following folding document is copied from #1577 and are closely related to this pull request:

Concealer noise

The NormalDistribution<nav_msgs::msg::Odometry> has been adjusted to the changes mentioned above.

The noise adding framework from concealer::Publisher has been used implementing additional concealer noise, that is noise applied to topics

• /vehicle/status/velocity_status
• /localization/pose_estimator/pose_with_covariance

For this reason, the NormalDistribution<autoware_vehicle_msgs::msg::VelocityReport> and NormalDistribution<geometry_msgs::msg::PoseWithCovarianceStamped> have been implemented and used in AutowareUniverse class.

Additionally, the unit tests for new NormalDistribution specializations have been implemented.

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

Fix rotation bug

The original implementation applies rotation roll-pitch-yaw order, but yaw-pitch-roll order is widely used in ROS context.
And in implementations where noise is applied to each axis independently, applying noise to multiple axes may affect other axes.
So, in this pull-request, I applies rotation noise using quaternion with tf2 library functions.

Stop publishing /tf when simulate_localization=false

If simulate_localization is set to false, the /tf for base_link is published from /localization/pose_twist_fusion_filter/ekf_localizer node.
So, I fix scenario_simulator to change /tf publishing target from base_link to base_link_ground_truth then.

covariance noise

Add noises for covariance diagnal elements.
The parameter naming convention can be extended beyond the diagonal elements.

parameters

You can set the additive/multiplicative errors with specifying mean/standard_deviation parameters for position , orientation and covariance.
The position parameters are local_x, local_y and local_z, and the orientation parameters are r, p and y for ease of use.

Details

    /localization/pose_estimator/pose_with_covariance:
      version: 20240605 # architecture_type suffix (mandatory)
      seed: 0 # If 0 is specified, a random seed value will be generated for each run.
      geometry_msgs::msg::PoseWithCovarianceStamped:
        pose:
          pose:
            position:
              local_x:
                error:
                  additive:
                    mean: 0.0
                    standard_deviation: 0.0
                  multiplicative:
                    mean: 0.0
                    standard_deviation: 0.0
              local_y:
                error:
                  additive:
                    mean: 0.0
                    standard_deviation: 0.0
                  multiplicative:
                    mean: 0.0
                    standard_deviation: 0.0
              local_z:
                error:
                  additive:
                    mean: 0.0
                    standard_deviation: 0.0
                  multiplicative:
                    mean: 0.0
                    standard_deviation: 0.0
            orientation:
              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
          covariance:
            x_x:
              error:
                additive:
                  mean: 0.0225
                  standard_deviation: 0.0
                multiplicative:
                  mean: 0.0
                  standard_deviation: 0.0
            y_y:
              error:
                additive:
                  mean: 0.0225
                  standard_deviation: 0.0
                multiplicative:
                  mean: 0.0
                  standard_deviation: 0.0
            z_z:
              error:
                additive:
                  mean: 0.0225
                  standard_deviation: 0.0
                multiplicative:
                  mean: 0.0
                  standard_deviation: 0.0
            roll_roll:
              error:
                additive:
                  mean: 0.000625
                  standard_deviation: 0.0
                multiplicative:
                  mean: 0.0
                  standard_deviation: 0.0
            pitch_pitch:
              error:
                additive:
                  mean: 0.000625
                  standard_deviation: 0.0
                multiplicative:
                  mean: 0.0
                  standard_deviation: 0.0
            yaw_yaw:
              error:
                additive:
                  mean: 0.000625
                  standard_deviation: 0.0
                multiplicative:
                  mean: 0.0
                  standard_deviation: 0.0

References

Regression Test: OK

Destructive Changes

None
The default parameter is zero.
So the default behavior is not changed

Known Limitations

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[github-actions]

This PR has failed the SonarQube scan. Please check the details in the SonarQube dashboard.

[github-actions]

Failure optional scenarios

Note

This is an experimental check and does not block merging the pull-request.
But, please be aware that this may indicate a regression.

scenario failed: execution_time_test

      <failure type="SimulationFailure" message="CustomCommandAction typed &quot;exitFailure&quot; was triggered by the named Conditions {&quot;update time checker&quot;, &quot;avoid startup&quot;}: {&quot;update time checker&quot;: Is the /simulation/interpreter/execution_time/update (= 0.005144000000000000134836586341) is greaterThan 0.005?}, {&quot;avoid startup&quot;: Is the simulation time (= 2.299999999999999822364316059975) is greaterThan 1.000000000000000000000000000000?}" />

[github-actions]

Failure optional scenarios

Note

This is an experimental check and does not block merging the pull-request.
But, please be aware that this may indicate a regression.

scenario failed: execution_time_test

      <failure type="SimulationFailure" message="CustomCommandAction typed &quot;exitFailure&quot; was triggered by the named Conditions {&quot;evaluate time checker&quot;, &quot;avoid startup&quot;}: {&quot;evaluate time checker&quot;: Is the /simulation/interpreter/execution_time/evaluate (= 0.005166999999999999718114374048) is greaterThan 0.001?}, {&quot;avoid startup&quot;: Is the simulation time (= 1.850000000000000976996261670138) is greaterThan 1.000000000000000000000000000000?}" />

[sonarqubecloud]

Quality Gate passed

Issues
3 New issues
0 Accepted issues

Measures
0 Security Hotspots
74.2% Coverage on New Code
12.1% Duplication on New Code

See analysis details on SonarQube Cloud