feat: Enhance IMU sensor configuration and initialization

 - Added frame_id to ImuSensorConfiguration
 - Separated noise standard deviations for orientation, twist, and acceleration
 - Updated ImuSensorBase and ImuSensor classes for new noise distributions

Author: SzymonParapura

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -337,11 +337,13 @@ class SimulatorCore
         core->attachImuSensor(entity_ref, [&]() {
           simulation_api_schema::ImuSensorConfiguration configuration;
           configuration.set_entity(entity_ref);
+          configuration.set_frame_id("tamagawa/imu_link");
           configuration.set_add_gravity(true);
-          configuration.set_add_noise(false);
           configuration.set_use_seed(true);
           configuration.set_seed(0);
-          configuration.set_noise_standard_deviation(0.01);
+          configuration.set_noise_standard_deviation_orientation(0.01);
+          configuration.set_noise_standard_deviation_twist(0.01);
+          configuration.set_noise_standard_deviation_acceleration(0.01);
           return configuration;
         }());
 

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

@@ -17,6 +17,7 @@
 
 #include <simulation_api_schema.pb.h>
 
+#include <array>
 #include <geometry_msgs/msg/accel.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <random>
@@ -32,10 +33,16 @@ class ImuSensorBase
 public:
   explicit ImuSensorBase(const simulation_api_schema::ImuSensorConfiguration & configuration)
   : add_gravity_(configuration.add_gravity()),
-    add_noise_(configuration.add_noise()),
-    noise_standard_deviation_(add_noise_ ? configuration.noise_standard_deviation() : 0.0),
+    noise_standard_deviation_orientation_(configuration.noise_standard_deviation_orientation()),
+    noise_standard_deviation_twist_(configuration.noise_standard_deviation_twist()),
+    noise_standard_deviation_acceleration_(configuration.noise_standard_deviation_acceleration()),
     random_generator_(configuration.use_seed() ? configuration.seed() : std::random_device{}()),
-    noise_distribution_(0.0, noise_standard_deviation_)
+    noise_distribution_orientation_(0.0, noise_standard_deviation_orientation_),
+    noise_distribution_twist_(0.0, noise_standard_deviation_twist_),
+    noise_distribution_acceleration_(0.0, noise_standard_deviation_acceleration_),
+    orientation_covariance_(calculateCovariance(noise_standard_deviation_orientation_)),
+    angular_velocity_covariance_(calculateCovariance(noise_standard_deviation_twist_)),
+    linear_acceleration_covariance_(calculateCovariance(noise_standard_deviation_acceleration_))
   {
   }
 
@@ -47,10 +54,21 @@ class ImuSensorBase
 
 protected:
   const bool add_gravity_;
-  const bool add_noise_;
-  const double noise_standard_deviation_;
+  const double noise_standard_deviation_orientation_;
+  const double noise_standard_deviation_twist_;
+  const double noise_standard_deviation_acceleration_;
   mutable std::mt19937 random_generator_;
-  mutable std::normal_distribution<> noise_distribution_;
+  mutable std::normal_distribution<> noise_distribution_orientation_;
+  mutable std::normal_distribution<> noise_distribution_twist_;
+  mutable std::normal_distribution<> noise_distribution_acceleration_;
+  const std::array<double, 9> orientation_covariance_;
+  const std::array<double, 9> angular_velocity_covariance_;
+  const std::array<double, 9> linear_acceleration_covariance_;
+
+  auto calculateCovariance(const double stddev) const -> std::array<double, 9>
+  {
+    return {std::pow(stddev, 2), 0, 0, 0, std::pow(stddev, 2), 0, 0, 0, std::pow(stddev, 2)};
+  };
 };
 
 template <typename MessageType>
@@ -60,7 +78,10 @@ class ImuSensor : public ImuSensorBase
   explicit ImuSensor(
     const simulation_api_schema::ImuSensorConfiguration & configuration,
     const typename rclcpp::Publisher<MessageType>::SharedPtr & publisher)
-  : ImuSensorBase(configuration), entity_name_(configuration.entity()), publisher_(publisher)
+  : ImuSensorBase(configuration),
+    entity_name_(configuration.entity()),
+    frame_id_(configuration.frame_id()),
+    publisher_(publisher)
   {
   }
 
@@ -85,6 +106,7 @@ class ImuSensor : public ImuSensorBase
     const traffic_simulator_msgs::msg::EntityStatus & status) const -> const MessageType;
 
   const std::string entity_name_;
+  const std::string frame_id_;
   const typename rclcpp::Publisher<MessageType>::SharedPtr publisher_;
 };
 }  // namespace simple_sensor_simulator

simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp

@@ -16,6 +16,7 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/LinearMath/Transform.h>
 
+#include <algorithm>
 #include <geometry/quaternion/euler_to_quaternion.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
 #include <simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp>
@@ -27,25 +28,30 @@ auto ImuSensor<sensor_msgs::msg::Imu>::generateMessage(
   const rclcpp::Time & current_ros_time,
   const traffic_simulator_msgs::msg::EntityStatus & status) const -> const sensor_msgs::msg::Imu
 {
-  const auto applyNoise = [&](geometry_msgs::msg::Vector3 & v) {
-    v.x += noise_distribution_(random_generator_);
-    v.y += noise_distribution_(random_generator_);
-    v.z += noise_distribution_(random_generator_);
-  };
+  const auto applyNoise =
+    [&](geometry_msgs::msg::Vector3 & v, std::normal_distribution<> & distribution) {
+      v.x += distribution(random_generator_);
+      v.y += distribution(random_generator_);
+      v.z += distribution(random_generator_);
+    };
 
   auto imu_msg = sensor_msgs::msg::Imu();
   imu_msg.header.stamp = current_ros_time;
-  imu_msg.header.frame_id = "tamagawa/imu_link";
+  imu_msg.header.frame_id = frame_id_;
 
   auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(status.pose.orientation);
   auto twist = status.action_status.twist;
   auto accel = status.action_status.accel;
 
   // Apply noise
-  if (add_noise_) {
-    applyNoise(orientation_rpy);
-    applyNoise(twist.angular);
-    applyNoise(accel.linear);
+  if (noise_standard_deviation_orientation_ > 0.0) {
+    applyNoise(orientation_rpy, noise_distribution_orientation_);
+  }
+  if (noise_standard_deviation_twist_ > 0.0) {
+    applyNoise(twist.angular, noise_distribution_twist_);
+  }
+  if (noise_standard_deviation_acceleration_ > 0.0) {
+    applyNoise(accel.linear, noise_distribution_acceleration_);
   }
 
   // Apply gravity
@@ -70,15 +76,15 @@ auto ImuSensor<sensor_msgs::msg::Imu>::generateMessage(
   imu_msg.linear_acceleration.z = accel.linear.z;
 
   // Set covariance
-  imu_msg.orientation_covariance = {std::pow(noise_standard_deviation_, 2), 0, 0, 0,
-                                    std::pow(noise_standard_deviation_, 2), 0, 0, 0,
-                                    std::pow(noise_standard_deviation_, 2)};
-  imu_msg.angular_velocity_covariance = {std::pow(noise_standard_deviation_, 2), 0, 0, 0,
-                                         std::pow(noise_standard_deviation_, 2), 0, 0, 0,
-                                         std::pow(noise_standard_deviation_, 2)};
-  imu_msg.linear_acceleration_covariance = {std::pow(noise_standard_deviation_, 2), 0, 0, 0,
-                                            std::pow(noise_standard_deviation_, 2), 0, 0, 0,
-                                            std::pow(noise_standard_deviation_, 2)};
+  std::copy(
+    orientation_covariance_.begin(), orientation_covariance_.end(),
+    imu_msg.orientation_covariance.data());
+  std::copy(
+    angular_velocity_covariance_.begin(), angular_velocity_covariance_.end(),
+    imu_msg.angular_velocity_covariance.data());
+  std::copy(
+    linear_acceleration_covariance_.begin(), linear_acceleration_covariance_.end(),
+    imu_msg.linear_acceleration_covariance.data());
   return imu_msg;
 }
 }  // namespace simple_sensor_simulator

simulation/simulation_interface/proto/simulation_api_schema.proto

@@ -37,12 +37,14 @@ message PseudoTrafficLightDetectorConfiguration {
  * Parameter configuration of the imu sensor
  **/
  message ImuSensorConfiguration {
-  string entity = 1;                   // Name of the entity which you want to attach imu.
-  bool add_gravity = 2;                // If true, gravity will be added to the acceleration vector
-  bool add_noise = 3;                  // If true, noise will be added to every vector published
-  bool use_seed = 4;                   // If true, as seed will be used the passed value, if not it will be random
-  int32 seed = 5;                      // Seed for random number generator
-  double noise_standard_deviation = 6; // Standard deviation for noise (normal distribution, mean equal to 0.0)
+  string entity = 1;                                // Name of the entity which you want to attach imu.
+  string frame_id = 2;                              // Frame ID for the IMU sensor
+  bool add_gravity = 3;                             // If true, gravity will be added to the acceleration vector
+  bool use_seed = 4;                                // If true, as seed will be used the passed value, if not it will be random
+  int32 seed = 5;                                   // Seed for random number generator
+  double noise_standard_deviation_orientation = 6;  // The standard deviation for orientation noise (normal distribution, mean = 0.0)
+  double noise_standard_deviation_twist = 7;        // The standard deviation for angular velocity noise (normal distribution, mean = 0.0)
+  double noise_standard_deviation_acceleration = 8; // The standard deviation for linear acceleration noise (normal distribution, mean = 0.0)
 }
 
 /**