Add imu covariance setting (#337)

Add imu covariance setting

Signed-off-by: Adam Krawczyk <[email protected]>
Signed-off-by: Khasreto <[email protected]>
Co-authored-by: Michał Pełka <[email protected]>

Gems/ROS2/Code/Include/ROS2/Utilities/ROS2Conversions.h

@@ -27,5 +27,6 @@ namespace ROS2
         AZ::Transform FromROS2Pose(const geometry_msgs::msg::Pose& ros2pose);
         AZ::Vector3 FromROS2Point(const geometry_msgs::msg::Point& ros2point);
         AZ::Quaternion FromROS2Quaternion(const geometry_msgs::msg::Quaternion& ros2quaternion);
+        std::array<double, 9> ToROS2Covariance(const AZ::Matrix3x3& covariance);
     }; // namespace ROS2Conversions
 } // namespace ROS2

Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp

@@ -37,7 +37,10 @@ namespace ROS2
                 ->Version(1)
                 ->Field("FilterSize", &ROS2ImuSensorComponent::m_filterSize)
                 ->Field("IncludeGravity", &ROS2ImuSensorComponent::m_includeGravity)
-                ->Field("AbsoluteRotation", &ROS2ImuSensorComponent::m_absoluteRotation);
+                ->Field("AbsoluteRotation", &ROS2ImuSensorComponent::m_absoluteRotation)
+                ->Field("AccelerationVariance", &ROS2ImuSensorComponent::m_linearAccelerationVariance)
+                ->Field("AngularVelocityVariance", &ROS2ImuSensorComponent::m_angularVelocityVariance)
+                ->Field("OrientationVariance", &ROS2ImuSensorComponent::m_orientationVariance);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -59,7 +62,22 @@ namespace ROS2
                         AZ::Edit::UIHandlers::Default,
                         &ROS2ImuSensorComponent::m_absoluteRotation,
                         "Absolute Rotation",
-                        "Include Absolute rotation in message.");
+                        "Include Absolute rotation in message.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ROS2ImuSensorComponent::m_linearAccelerationVariance,
+                        "Linear Acceleration Variance",
+                        "Variance of linear acceleration.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ROS2ImuSensorComponent::m_angularVelocityVariance,
+                        "Angular Velocity Variance",
+                        "Variance of angular velocity.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ROS2ImuSensorComponent::m_orientationVariance,
+                        "Orientation Variance",
+                        "Variance of orientation.");
             }
         }
     }
@@ -120,10 +138,14 @@ namespace ROS2
                 m_acceleration += inv.TransformVector(gravity);
             }
             m_imuMsg.linear_acceleration = ROS2Conversions::ToROS2Vector3(m_acceleration);
+            m_imuMsg.linear_acceleration_covariance =  ROS2Conversions::ToROS2Covariance(m_linearAccelerationCovariance);
             m_imuMsg.angular_velocity = ROS2Conversions::ToROS2Vector3(angularRateFiltered);
+            m_imuMsg.angular_velocity_covariance = ROS2Conversions::ToROS2Covariance(m_angularVelocityCovariance);
+
             if (m_absoluteRotation)
             {
                 m_imuMsg.orientation = ROS2Conversions::ToROS2Quaternion(rigidbody->GetTransform().GetRotation());
+                m_imuMsg.orientation_covariance = ROS2Conversions::ToROS2Covariance(m_orientationCovariance);
             }
             m_imuMsg.header.stamp = ROS2Interface::Get()->GetROSTimestamp();
             this->m_imuPublisher->publish(m_imuMsg);
@@ -138,6 +160,11 @@ namespace ROS2
         const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImuMsgType];
         const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
         m_imuPublisher = ros2Node->create_publisher<sensor_msgs::msg::Imu>(fullTopic.data(), publisherConfig.GetQoS());
+        
+        m_linearAccelerationCovariance = toCovarianceMatrix(m_linearAccelerationVariance);
+        m_angularVelocityCovariance = toCovarianceMatrix(m_angularVelocityVariance);
+        m_orientationCovariance = toCovarianceMatrix(m_orientationVariance);
+        
         ROS2SensorComponent::Activate();
     }
 
@@ -148,4 +175,13 @@ namespace ROS2
         m_imuPublisher.reset();
     }
 
+    AZ::Matrix3x3 ROS2ImuSensorComponent::ToDiagonalCovarianceMatrix(const AZ::Vector3& variance)
+    {
+        AZ::Matrix3x3 covarianceMatrix;
+        covarianceMatrix.SetElement(0, 0, Variance.GetX());
+        covarianceMatrix.SetElement(1, 1, Variance.GetY());
+        covarianceMatrix.SetElement(2, 2, Variance.GetZ());
+        return covarianceMatrix;
+    }
+
 } // namespace ROS2

Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h

@@ -55,11 +55,22 @@ namespace ROS2
         AZStd::deque<AZ::Vector3> m_filterAcceleration;
         AZStd::deque<AZ::Vector3> m_filterAngularVelocity;
 
+        AZ::Vector3 m_orientationVariance = AZ::Vector3::CreateZero();
+        AZ::Vector3 m_angularVelocityVariance = AZ::Vector3::CreateZero();
+        AZ::Vector3 m_linearAccelerationVariance = AZ::Vector3::CreateZero();
+
+
+        AZ::Matrix3x3 m_orientationCovariance = AZ::Matrix3x3::CreateZero();
+        AZ::Matrix3x3 m_angularVelocityCovariance = AZ::Matrix3x3::CreateZero();
+        AZ::Matrix3x3 m_linearAccelerationCovariance = AZ::Matrix3x3::CreateZero();
+
     private:
         // ROS2SensorComponent overrides ...
         void SetupRefreshLoop() override;
 
         // ROS2::Utils::PhysicsCallbackHandler overrides ...
         void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override;
+
+        AZ::Matrix3x3 toCovarianceMatrix(const AZ::Vector3& Variance);
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Utilities/ROS2Conversions.cpp

@@ -71,4 +71,15 @@ namespace ROS2
         azquaternion.SetW(ros2quaternion.w);
         return azquaternion;
     }
+
+    std::array<double, 9> ROS2Conversions::ToROS2Covariance(const AZ::Matrix3x3& covariance)
+    {
+        std::array<double, 9> ros2Covariance;
+        for (int i = 0; i < 9; ++i)
+        {
+            ros2Covariance[i] = covariance.GetElement(i / 3, i % 3);
+        }
+        return ros2Covariance;
+    }
+
 } // namespace ROS2