diff --git a/Runtime/Tx/Msgs/Nav/OdometryTx.cs b/Runtime/Tx/Msgs/Nav/OdometryTx.cs index 522377e..3703235 100644 --- a/Runtime/Tx/Msgs/Nav/OdometryTx.cs +++ b/Runtime/Tx/Msgs/Nav/OdometryTx.cs @@ -78,11 +78,16 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts) data.pose.pose.position.x += GaussianNoise.Generate(positionNoiseStdDev); data.pose.pose.position.y += GaussianNoise.Generate(positionNoiseStdDev); data.pose.pose.position.z += GaussianNoise.Generate(positionNoiseStdDev); + + var qNoise = Quaternion.Euler( + GaussianNoise.Generate(rotationNoiseStdDev), + GaussianNoise.Generate(rotationNoiseStdDev), + GaussianNoise.Generate(rotationNoiseStdDev) + ); + + var orientationWithNoise = data.pose.pose.orientation.FromRos() * qNoise; - data.pose.pose.orientation.x += GaussianNoise.Generate(rotationNoiseStdDev); - data.pose.pose.orientation.y += GaussianNoise.Generate(rotationNoiseStdDev); - data.pose.pose.orientation.z += GaussianNoise.Generate(rotationNoiseStdDev); - data.pose.pose.orientation.w += GaussianNoise.Generate(rotationNoiseStdDev); + data.pose.pose.orientation = orientationWithNoise.ToRos(); data.twist.twist.linear.x += GaussianNoise.Generate(linearVelocityNoiseStdDev); data.twist.twist.linear.y += GaussianNoise.Generate(linearVelocityNoiseStdDev);