Improve IMU NormalDistribution deactivation

Skip the randomization process altogether instead of disabling all errors and performing other operations that achieve nothing

Signed-off-by: Mateusz Palczuk <[email protected]>

Author: TauTheLepton

external/concealer/include/concealer/publisher.hpp

@@ -45,9 +45,6 @@ struct NormalDistributionError
 {
   static_assert(std::is_floating_point_v<std::decay_t<ValueType> >, "Unsupported error type");
 
-  /// @note set this to false to disable randomization
-  bool active{true};
-
   std::normal_distribution<ValueType> additive, multiplicative;
 
   explicit NormalDistributionError(
@@ -66,11 +63,7 @@ struct NormalDistributionError
 
   auto apply(std::mt19937_64 & engine, const ValueType value) -> ValueType
   {
-    if (active) {
-      return value * (multiplicative(engine) + static_cast<ValueType>(1)) + additive(engine);
-    } else {
-      return value;
-    }
+    return value * (multiplicative(engine) + static_cast<ValueType>(1)) + additive(engine);
   }
 };
 
@@ -171,11 +164,12 @@ struct NormalDistribution<sensor_msgs::msg::Imu> : public RandomNumberEngine
                                   linear_acceleration_z_error;
   // clang-format on
 
+  /// @note set this to false to disable randomization
+  bool active{true};
+
   explicit NormalDistribution(
     const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &, const std::string &);
 
-  auto deactivate() -> void;
-
   auto operator()(sensor_msgs::msg::Imu imu) -> sensor_msgs::msg::Imu;
 };
 

external/concealer/src/publisher.cpp

@@ -215,22 +215,13 @@ NormalDistribution<sensor_msgs::msg::Imu>::NormalDistribution(
 {
 }
 
-auto NormalDistribution<sensor_msgs::msg::Imu>::deactivate() -> void
-{
-  orientation_r_error.active = false;
-  orientation_p_error.active = false;
-  orientation_y_error.active = false;
-  angular_velocity_x_error.active = false;
-  angular_velocity_y_error.active = false;
-  angular_velocity_z_error.active = false;
-  linear_acceleration_x_error.active = false;
-  linear_acceleration_y_error.active = false;
-  linear_acceleration_z_error.active = false;
-}
-
 auto NormalDistribution<sensor_msgs::msg::Imu>::operator()(sensor_msgs::msg::Imu imu)
   -> sensor_msgs::msg::Imu
 {
+  if (not active) {
+    return imu;
+  }
+
   imu.orientation = math::geometry::convertEulerAngleToQuaternion([this, &imu] {
     auto rpy = math::geometry::convertQuaternionToEulerAngle(imu.orientation);
 

simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp

@@ -99,7 +99,7 @@ class ImuSensor : public ImuSensorBase
      * If legacy is not overriden we don't want to recalculate covariance matrices, so return early
      */
     if (not override_legacy_configuration_) {
-      publish.getMutableRandomizer().deactivate();
+      publish.getMutableRandomizer().active = false;
       return;
     }