Feature: Add noise to Localization, Velocity status and IMU #1577
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
…ionError and make it a template to accept multiple floating point types Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
The previous noise mechanism is not in use This required accessing Publisher::randomize to obtain normal distribution error standard deviations Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Test applying noise using legacy parameters - simulation_api_schema Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
…arameters file Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Checklist for reviewers ☑️All references to "You" in the following text refer to the code reviewer.
|
Signed-off-by: Mateusz Palczuk <[email protected]>
TauTheLepton
added
feature
TauTheLepton
changed the title
|
No regressions confirmed here. |
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Signed-off-by: Mateusz Palczuk <[email protected]>
Failure optional scenariosNote This is an experimental check and does not block merging the pull-request. scenario failed: execution_time_test <failure type="SimulationFailure" message="CustomCommandAction typed "exitFailure" was triggered by the named Conditions {"update time checker", "avoid startup"}: {"update time checker": Is the /simulation/interpreter/execution_time/update (= 0.005168999999999999983568699236) is greaterThan 0.005?}, {"avoid startup": Is the simulation time (= 9.449999999999999289457264239900) is greaterThan 1.000000000000000000000000000000?}" /> |
Skip the randomization process altogether instead of disabling all errors and performing other operations that achieve nothing Signed-off-by: Mateusz Palczuk <[email protected]>
… non-mutable Adjust ImuSensor + ImuSensorBase update member functions to non-const - similar to other sensors Signed-off-by: Mateusz Palczuk <[email protected]>
|
I have applied the changes requested. While doing this, I have realized that the deactivation of IMU sensor randomization - when legacy configuration is used - is done inefficiently, so I have improved this by skipping the whole randomization altogether. For details, please see the commit dce11aa. |
Signed-off-by: Mateusz Palczuk <[email protected]>
Failure optional scenariosNote This is an experimental check and does not block merging the pull-request. scenario failed: execution_time_test <failure type="SimulationFailure" message="CustomCommandAction typed "exitFailure" was triggered by the named Conditions {"update time checker", "avoid startup"}: {"update time checker": Is the /simulation/interpreter/execution_time/update (= 0.007413000000000000318245430009) is greaterThan 0.005?}, {"avoid startup": Is the simulation time (= 12.300000000000039790393202565610) is greaterThan 1.000000000000000000000000000000?}" /> |
Ensure variances (diagonal) are not 0.0, or otherwise Autoware localization gets confused (only when simulate_localization:=false) Signed-off-by: Mateusz Palczuk <[email protected]>
This reverts commit c729976. Signed-off-by: Mateusz Palczuk <[email protected]>
…in legacy configuration This was done so that covariance matrices are calculated to be non-zero (their diagonal). Otherwise the Autoware gets confused and Ego starts shaking in RViz. Signed-off-by: Mateusz Palczuk <[email protected]>
|
@yamacir-kit |
TauTheLepton
force-pushed
the
feature/publisher-noise
branch
from
fa7b459 to
c3fc045
Compare
… used Signed-off-by: Mateusz Palczuk <[email protected]>
TauTheLepton
force-pushed
the
feature/publisher-noise
branch
from
c3fc045 to
2d3fb38
Compare
|
No regressions confirmed here |
Failure optional scenariosNote This is an experimental check and does not block merging the pull-request. scenario failed: execution_time_test <failure type="SimulationFailure" message="CustomCommandAction typed "exitFailure" was triggered by the named Conditions {"update time checker", "avoid startup"}: {"update time checker": Is the /simulation/interpreter/execution_time/update (= 0.005033000000000000147271084217) is greaterThan 0.005?}, {"avoid startup": Is the simulation time (= 8.649999999999987920773492078297) is greaterThan 1.000000000000000000000000000000?}" /> |
|
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Description
Abstract
This pull request aims to add noise to the topics:
/vehicle/status/velocity_status/sensing/imu/imu_data/localization/pose_estimator/pose_with_covarianceNote
This pull request may seem to be big, but the majority of the PR are tests and appending the parameters file.
For details on what part of this PR is not changing any functionality or actual simulator code, please see the table below.
Background
Details
Publisher noise framework refactoring
The concealer noise framework has been slightly refactored in order to make it easier to implement additional randomizers.
The struct
Errordefined inNormalDistribution<nav_msgs::msg::Odometry>has been moved outside theNormalDistribution<nav_msgs::msg::Odometry>and renamed toNormalDistributionError, so that it could be used in otherNormalDistributionspecializations.It has also been converted to template so that other real types supported by
std::normal_distributioncould be used.Additionally, a member
activehas been added, so that the randomization of data could be turned off (this is opt-in, default is always active).NormalDistributionBasebase struct has been implemented in order to provide common functionality for all normalNormalDistributionspecializations. It providesseedandenginemembers and a constructor that initializes them. Theseedis obtained from the appropriate seed ROS parameter, andengineis initialized using theseed.Thanks to the
NormalDistributionBase,NormalDistributionspecializations only have to define what randomization errors they contain, how they are initialized and how they are applied.What is more, the
Publisher::randomizemember has been declared mutable, because its internal state changes (the random number generator engine) and this should not block developers from creatingconstPublishers.Concealer noise
The
NormalDistribution<nav_msgs::msg::Odometry>has been adjusted to the changes mentioned above.The noise adding framework from
concealer::Publisherhas been used implementing additional concealer noise, that is noise applied to topics/vehicle/status/velocity_status/localization/pose_estimator/pose_with_covarianceFor this reason, the
NormalDistribution<autoware_vehicle_msgs::msg::VelocityReport>andNormalDistribution<geometry_msgs::msg::PoseWithCovarianceStamped>have been implemented and used inAutowareUniverseclass.Additionally, the unit tests for new
NormalDistributionspecializations have been implemented.Simple sensor simulator noise
Noise adding framework from
concealer::Publisherhas been used implementing simple sensor simulator noise, that is the noise applied to topic/sensing/imu/imu_dataApplying 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 from
concealer::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_configurationto 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
ImuSensorwith both noise configurations (legacy and new) have been implemented.Parameters
Finally, all parameters used for implemented noise have been listed in the scenario test runner parameters file.
References
INTERNAL LINK
No regressions confirmed
Destructive Changes
None
Known Limitations
None