18.4.0

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

Related Issues